From f5e3e46601064f79790662472b0f531e8f936182 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 3 Nov 2026 15:59:29 -0800 Subject: [PATCH 1/2] Update to latest version of ros2_control API --- .github/workflows/release.yml | 53 ++++++++ .pre-commit-config.yaml | 12 +- auv_control_demos/CHANGELOG.md | 2 + auv_control_demos/package.xml | 2 +- auv_control_msgs/CHANGELOG.md | 2 + auv_control_msgs/package.xml | 2 +- auv_controllers/CHANGELOG.md | 2 + auv_controllers/package.xml | 2 +- controller_common/CHANGELOG.md | 4 + controller_common/CMakeLists.txt | 4 +- .../include/controller_common/common.hpp | 20 +++ controller_common/package.xml | 3 +- controller_common/src/common.cpp | 37 ++++++ controller_coordinator/CHANGELOG.md | 2 + controller_coordinator/CMakeLists.txt | 12 +- controller_coordinator/package.xml | 2 +- ik_solvers/CHANGELOG.md | 2 + ik_solvers/package.xml | 2 +- ik_solvers/src/task_priority_solver.cpp | 5 +- impedance_controller/CHANGELOG.md | 5 + .../impedance_controller.hpp | 11 +- impedance_controller/package.xml | 2 +- .../src/impedance_controller.cpp | 82 ++++++------ .../CHANGELOG.md | 5 + .../thruster_allocation_matrix_controller.hpp | 9 +- .../package.xml | 2 +- .../thruster_allocation_matrix_controller.cpp | 51 ++++---- thruster_controllers/CHANGELOG.md | 6 + thruster_controllers/CMakeLists.txt | 3 + .../gz_passthrough_controller.hpp | 6 +- .../polynomial_thrust_curve_controller.hpp | 6 +- .../rotation_rate_controller.hpp | 6 +- thruster_controllers/package.xml | 2 +- .../src/gz_passthrough_controller.cpp | 42 ++++--- .../polynomial_thrust_curve_controller.cpp | 43 +++---- .../src/rotation_rate_controller.cpp | 42 ++++--- topic_sensors/CHANGELOG.md | 5 + .../include/topic_sensors/odom_sensor.hpp | 10 +- topic_sensors/package.xml | 2 +- topic_sensors/src/odom_sensor.cpp | 60 +++++---- trajectory_controllers/CHANGELOG.md | 5 + .../cartesian_trajectory_controller.hpp | 29 +++-- trajectory_controllers/package.xml | 2 +- .../src/cartesian_trajectory_controller.cpp | 119 +++++++++++------- twist_controllers/CHANGELOG.md | 5 + ...egral_terminal_sliding_mode_controller.hpp | 13 +- .../integral_sliding_mode_controller.hpp | 13 +- twist_controllers/package.xml | 2 +- ...egral_terminal_sliding_mode_controller.cpp | 76 ++++++----- .../src/integral_sliding_mode_controller.cpp | 75 ++++++----- whole_body_controllers/CHANGELOG.md | 5 + .../whole_body_controllers/ik_controller.hpp | 11 +- whole_body_controllers/package.xml | 2 +- whole_body_controllers/src/ik_controller.cpp | 70 ++++++----- 54 files changed, 655 insertions(+), 342 deletions(-) create mode 100644 .github/workflows/release.yml diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..93f2a81 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,53 @@ +name: Release + +on: + push: + tags: + - "[0-9]+.[0-9]+.[0-9]+*" + +permissions: + contents: write + +jobs: + release: + name: Publish release + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v7 + with: + fetch-depth: 0 + + - name: Setup Python + uses: actions/setup-python@v7 + with: + python-version: "3.14" + + - name: Verify tag matches package versions + shell: python + run: | + import os + import pathlib + import sys + import xml.etree.ElementTree as ET + + tag = os.environ["GITHUB_REF_NAME"] + mismatched = [] + for manifest in sorted(pathlib.Path(".").glob("*/package.xml")): + version = ET.parse(manifest).getroot().findtext("version") + if version != tag: + mismatched.append((manifest, version)) + + for manifest, version in mismatched: + print(f"::error file={manifest}::Tag {tag} does not match package version {version}") + if mismatched: + sys.exit(1) + + - name: Create GitHub release + env: + GH_TOKEN: ${{ github.token }} + run: | + prerelease="" + case "${GITHUB_REF_NAME}" in *-*) prerelease="--prerelease" ;; esac + gh release create "${GITHUB_REF_NAME}" \ + --generate-notes --verify-tag ${prerelease} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e8d8a91..ffd9499 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,28 +1,28 @@ repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.0.291 + rev: v0.17.3 hooks: - id: ruff args: ["--fix", "--exit-non-zero-on-fix"] - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.4.3 hooks: - id: codespell - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v23.1.3 hooks: - id: clang-format types_or: [c++, c] - - repo: https://github.com/BlankSpruce/gersemi - rev: 0.21.0 + - repo: https://github.com/BlankSpruce/gersemi-pre-commit + rev: 0.29.2 hooks: - id: gersemi - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-case-conflict diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 2eaab9d..bf08e9c 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_demos +## 0.5.2 (2026-11-03) + ## 0.5.1 (2026-08-14) - Declares the missing `rclpy` dependency diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index da4003d..6ba380e 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.5.1 + 0.5.2 Example package that includes demos for using auv_controllers in individual and chained modes Evan Palmer diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index 37e41db..e26449a 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_control_msgs +## 0.5.2 (2026-11-03) + ## 0.5.1 (2026-08-14) - Declares the missing `builtin_interfaces` dependency diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 1b3b335..afdc482 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -3,7 +3,7 @@ auv_control_msgs - 0.5.1 + 0.5.2 Custom messages for AUV controllers Evan Palmer diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index 4019d8a..1b221a9 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_controllers +## 0.5.2 (2026-11-03) + ## 0.5.1 (2026-08-14) ## 0.5.0 (2026-05-29) diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index f00d941..20929d6 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.5.1 + 0.5.2 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index faeb0c8..cb6fafb 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package controller_common +## 0.5.2 (2026-11-03) + +- Adds `common::interfaces` helpers for creating, reading, and resetting reference interfaces + ## 0.5.1 (2026-08-14) ## 0.5.0 (2026-05-29) diff --git a/controller_common/CMakeLists.txt b/controller_common/CMakeLists.txt index 8d79054..0d077d4 100644 --- a/controller_common/CMakeLists.txt +++ b/controller_common/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(auv_control_msgs REQUIRED) +find_package(hardware_interface REQUIRED) add_library(controller_common SHARED) target_sources( @@ -31,6 +32,7 @@ target_link_libraries( ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS} ${auv_control_msgs_TARGETS} + hardware_interface::hardware_interface ) install( @@ -43,6 +45,6 @@ install( ) ament_export_targets(export_controller_common HAS_LIBRARY_TARGET) -ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs" "auv_control_msgs") +ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs" "auv_control_msgs" "hardware_interface") ament_package() diff --git a/controller_common/include/controller_common/common.hpp b/controller_common/include/controller_common/common.hpp index 15895fc..6de07b4 100644 --- a/controller_common/include/controller_common/common.hpp +++ b/controller_common/include/controller_common/common.hpp @@ -20,12 +20,14 @@ #pragma once +#include #include #include "auv_control_msgs/msg/impedance_command.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/wrench.hpp" +#include "hardware_interface/handle.hpp" #include "nav_msgs/msg/odometry.hpp" namespace common @@ -75,4 +77,22 @@ auto isclose(double a, double b, double rtol = 1e-05, double atol = 1e-08) -> bo } // namespace math +namespace interfaces +{ + +/// Create a reference interface `/` for a chainable controller, initialized to NaN. +auto make_reference_interface(const std::string & prefix, const std::string & name) + -> hardware_interface::CommandInterface::SharedPtr; + +/// Read the values of the interfaces into `values` without blocking. +/// Values that cannot be read (e.g., because the interface is locked) are left unchanged. +auto read_values( + const std::vector & interfaces, + std::vector & values) -> void; + +/// Write `value` to all interfaces without blocking. +auto fill(const std::vector & interfaces, double value) -> void; + +} // namespace interfaces + } // namespace common diff --git a/controller_common/package.xml b/controller_common/package.xml index dadbb1c..4bf3eb8 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.5.1 + 0.5.2 Common interfaces for controllers used in this project Evan Palmer @@ -20,6 +20,7 @@ geometry_msgs nav_msgs auv_control_msgs + hardware_interface ament_cmake diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp index 5903b80..575f6dc 100644 --- a/controller_common/src/common.cpp +++ b/controller_common/src/common.cpp @@ -22,6 +22,9 @@ #include #include +#include +#include +#include namespace common { @@ -207,4 +210,38 @@ auto isclose(double a, double b, double rtol, double atol) -> bool } // namespace math +namespace interfaces +{ + +auto make_reference_interface(const std::string & prefix, const std::string & name) + -> hardware_interface::CommandInterface::SharedPtr +{ + hardware_interface::InterfaceInfo info; + info.name = name; + auto interface = + std::make_shared(hardware_interface::InterfaceDescription(prefix, info)); + std::ignore = interface->set_value(nan); + return interface; +} + +auto read_values( + const std::vector & interfaces, + std::vector & values) -> void +{ + for (auto && [interface, value] : std::views::zip(interfaces, values)) { + if (const auto v = interface->get_optional(); v.has_value()) { + value = v.value(); + } + } +} + +auto fill(const std::vector & interfaces, double value) -> void +{ + for (const auto & interface : interfaces) { + std::ignore = interface->set_value(value); + } +} + +} // namespace interfaces + } // namespace common diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md index 1464936..5326df5 100644 --- a/controller_coordinator/CHANGELOG.md +++ b/controller_coordinator/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_coordinator +## 0.5.2 (2026-11-03) + ## 0.5.1 (2026-08-14) ## 0.5.0 (2026-05-29) diff --git a/controller_coordinator/CMakeLists.txt b/controller_coordinator/CMakeLists.txt index 4a92e15..1637e60 100644 --- a/controller_coordinator/CMakeLists.txt +++ b/controller_coordinator/CMakeLists.txt @@ -30,7 +30,17 @@ target_link_libraries( install( TARGETS controller_coordinator controller_coordinator_parameters - DESTINATION lib/controller_coordinator + EXPORT export_controller_coordinator + RUNTIME DESTINATION lib/controller_coordinator + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +ament_export_targets(export_controller_coordinator HAS_LIBRARY_TARGET) +ament_export_dependencies( + "rclcpp" + "controller_manager_msgs" + "std_srvs" ) ament_package() diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml index 6877179..4483ceb 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -3,7 +3,7 @@ controller_coordinator - 0.5.1 + 0.5.2 A high-level node used to load and activate/deactivate control systems Evan Palmer diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index 2ae5204..759d70a 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package ik_solvers +## 0.5.2 (2026-11-03) + ## 0.5.1 (2026-08-14) - Declares the `python3-numpy` build dependency required by Pinocchio's Python bindings diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index 7de6a5e..0093fa9 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.5.1 + 0.5.2 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp index 8fb91a5..5d92f29 100644 --- a/ik_solvers/src/task_priority_solver.cpp +++ b/ik_solvers/src/task_priority_solver.cpp @@ -337,8 +337,9 @@ auto TaskPriorityIKSolver::solve_ik(const Eigen::Isometry3d & goal, const Eigen: const double joint_limit_gain = params_.joint_limit_task.constrained_joints_map[joint_name].gain; // insert the joint limit constraint - hierarchy_.insert(std::make_shared( - model_, primal, ub, lb, tol, activation, joint_name, joint_limit_gain)); + hierarchy_.insert( + std::make_shared( + model_, primal, ub, lb, tol, activation, joint_name, joint_limit_gain)); } // find the safe solutions and choose the one with the smallest norm diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md index 910f64d..89ebe6b 100644 --- a/impedance_controller/CHANGELOG.md +++ b/impedance_controller/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package impedance_controller +## 0.5.2 (2026-11-03) + +- Exports reference interfaces with `on_export_reference_interfaces_list`, replacing the `on_export_reference_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` + ## 0.5.1 (2026-08-14) - Declares dependencies that were previously pulled in transitively diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp index 6590e03..918264e 100644 --- a/impedance_controller/include/impedance_controller/impedance_controller.hpp +++ b/impedance_controller/include/impedance_controller/impedance_controller.hpp @@ -28,8 +28,8 @@ #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // auto-generated by generate_parameter_library #include @@ -56,7 +56,7 @@ class ImpedanceController : public controller_interface::ChainableControllerInte -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -71,10 +71,13 @@ class ImpedanceController : public controller_interface::ChainableControllerInte // provide an interface for setting reference commands from a topic // this is helpful when we want to set the reference using a non-ros2_control interface - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; - realtime_tools::RealtimeBuffer system_state_; + // local copy of the reference interface values used to compute the control command + std::vector reference_values_; + + realtime_tools::RealtimeThreadSafeBox system_state_; std::shared_ptr> system_state_sub_; std::vector system_state_values_; diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml index 7df6476..3b62137 100644 --- a/impedance_controller/package.xml +++ b/impedance_controller/package.xml @@ -3,7 +3,7 @@ impedance_controller - 0.5.1 + 0.5.2 An impedance controller for underwater vehicles Evan Palmer diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index 090842e..ce70989 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -20,7 +20,9 @@ #include "impedance_controller/impedance_controller.hpp" +#include #include +#include #include #include "auv_control_msgs/msg/impedance_command.hpp" @@ -101,8 +103,8 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou { configure_parameters(); - reference_.writeFromNonRT(auv_control_msgs::msg::ImpedanceCommand()); - system_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + reference_.set(auv_control_msgs::msg::ImpedanceCommand()); + system_state_.set(nav_msgs::msg::Odometry()); command_interfaces_.reserve(n_command_dofs_); state_interfaces_.reserve(n_state_dofs_); @@ -114,7 +116,7 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); if (params_.use_external_measured_states) { @@ -123,7 +125,7 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou "~/system_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - system_state_.writeFromNonRT(*msg); + system_state_.set(*msg); }); } @@ -137,10 +139,16 @@ auto ImpedanceController::on_configure(const rclcpp_lifecycle::State & /*previou auto ImpedanceController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - common::messages::reset_message(reference_.readFromNonRT()); - common::messages::reset_message(system_state_.readFromNonRT()); + auv_control_msgs::msg::ImpedanceCommand reference; + common::messages::reset_message(&reference); + reference_.set(reference); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + nav_msgs::msg::Odometry system_state; + common::messages::reset_message(&system_state); + system_state_.set(system_state); + + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); + reference_values_.assign(reference_values_.size(), std::numeric_limits::quiet_NaN()); system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; @@ -181,36 +189,28 @@ auto ImpedanceController::state_interface_configuration() const -> controller_in return config; } -auto ImpedanceController::on_export_reference_interfaces() -> std::vector +auto ImpedanceController::on_export_reference_interfaces_list() + -> std::vector { // the reference command includes the desired pose, velocity, and force/torque - reference_interfaces_.resize(n_reference_dofs_, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); + reference_values_.resize(n_reference_dofs_, std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(n_reference_dofs_); // add the pose & velocity interfaces // this uses the same position/velocity joint names as the state interfaces for (const auto [i, dof] : std::views::enumerate(state_dofs_)) { - if (i < 7) { - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION), - &reference_interfaces_[i]); - } else { - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY), - &reference_interfaces_[i]); - } + const auto * type = i < 7 ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; + interfaces.push_back( + common::interfaces::make_reference_interface(get_node()->get_name(), std::format("{}/{}", dof, type))); } // add the force/torque interfaces // this uses the same effort joint names as the command interfaces - for (const auto [i, dof] : std::views::enumerate(command_dofs_)) { - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT), - &reference_interfaces_[i + n_state_dofs_]); + for (const auto & dof : command_dofs_) { + interfaces.push_back( + common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT))); } return interfaces; @@ -220,22 +220,25 @@ auto ImpedanceController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - const std::vector reference = common::messages::to_vector(*current_reference); - for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { - if (!std::isnan(ref)) { - interface = ref; + // consume the latest reference so that it is only applied once + reference_.try_set([this](auv_control_msgs::msg::ImpedanceCommand & reference) -> void { + const std::vector values = common::messages::to_vector(reference); + for (const auto & [interface, ref] : std::views::zip(ordered_exported_reference_interfaces_, values)) { + if (!std::isnan(ref)) { + std::ignore = interface->set_value(ref); + } } - } - common::messages::reset_message(current_reference); + common::messages::reset_message(&reference); + }); return controller_interface::return_type::OK; } auto ImpedanceController::update_system_state_values() -> controller_interface::return_type { if (params_.use_external_measured_states) { - auto * current_state = system_state_.readFromRT(); - std::ranges::copy(common::messages::to_vector(*current_state), system_state_values_.begin()); + system_state_.try_get([this](const nav_msgs::msg::Odometry & state) -> void { + std::ranges::copy(common::messages::to_vector(state), system_state_values_.begin()); + }); } else { std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); @@ -256,7 +259,8 @@ auto ImpedanceController::update_and_validate_interfaces() -> controller_interfa RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT return controller_interface::return_type::ERROR; } - if (common::math::has_nan(reference_interfaces_)) { + common::interfaces::read_values(ordered_exported_reference_interfaces_, reference_values_); + if (common::math::has_nan(reference_values_)) { RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } @@ -274,11 +278,11 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c configure_parameters(); // get the reference pose, twist, and wrench values - const auto pose_end = reference_interfaces_.begin() + 7; + const auto pose_end = reference_values_.begin() + 7; const auto twist_end = pose_end + 6; const auto wrench_end = twist_end + 6; - const std::vector ref_pose_values(reference_interfaces_.begin(), pose_end); + const std::vector ref_pose_values(reference_values_.begin(), pose_end); const std::vector ref_twist_values(pose_end, twist_end); const std::vector ref_wrench_values(twist_end, wrench_end); diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index 9649131..11f8845 100644 --- a/thruster_allocation_matrix_controller/CHANGELOG.md +++ b/thruster_allocation_matrix_controller/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package thruster_allocation_matrix_controller +## 0.5.2 (2026-11-03) + +- Exports reference interfaces with `on_export_reference_interfaces_list`, replacing the `on_export_reference_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` + ## 0.5.1 (2026-08-14) - Fixes a format-string bug in the controller's error logging diff --git a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp index 0dd56fd..fce39ae 100644 --- a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp +++ b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp @@ -30,8 +30,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "geometry_msgs/msg/wrench.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // auto-generated by generate_parameter_library #include @@ -59,7 +59,7 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -68,9 +68,12 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; + // local copy of the reference interface values used to compute the control command + std::vector reference_values_; + using ControllerState = auv_control_msgs::msg::MultiActuatorStateStamped; std::shared_ptr> controller_state_pub_; std::unique_ptr> rt_controller_state_pub_; diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index e7a3ed8..372ebaf 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.5.1 + 0.5.2 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp index ac9064f..0016078 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -23,7 +23,9 @@ #include #include #include +#include #include +#include #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -104,14 +106,14 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St return ret; } - reference_.writeFromNonRT(geometry_msgs::msg::Wrench()); + reference_.set(geometry_msgs::msg::Wrench()); command_interfaces_.reserve(n_thrusters_); reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); @@ -130,8 +132,11 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St auto ThrusterAllocationMatrixController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - common::messages::reset_message(reference_.readFromNonRT()); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + geometry_msgs::msg::Wrench reference; + common::messages::reset_message(&reference); + reference_.set(reference); + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); + reference_values_.assign(reference_values_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -162,18 +167,17 @@ auto ThrusterAllocationMatrixController::state_interface_configuration() const } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto ThrusterAllocationMatrixController::on_export_reference_interfaces() - -> std::vector +auto ThrusterAllocationMatrixController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); + reference_values_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(n_dofs_); for (const auto & dof : dofs_) { - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT), - &reference_interfaces_[interfaces.size()]); + interfaces.push_back( + common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_EFFORT))); } return interfaces; @@ -183,14 +187,16 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - const std::vector wrench = common::messages::to_vector(*current_reference); - if (!std::ranges::all_of(wrench, [](double x) -> bool { return std::isnan(x); })) { - for (auto && [interface, ref] : std::views::zip(reference_interfaces_, wrench)) { - interface = std::isnan(ref) ? 0.0 : ref; + // consume the latest reference so that it is only applied once + reference_.try_set([this](geometry_msgs::msg::Wrench & reference) -> void { + const std::vector wrench = common::messages::to_vector(reference); + if (!common::math::all_nan(wrench)) { + for (const auto & [interface, ref] : std::views::zip(ordered_exported_reference_interfaces_, wrench)) { + std::ignore = interface->set_value(std::isnan(ref) ? 0.0 : ref); + } } - } - common::messages::reset_message(current_reference); + common::messages::reset_message(&reference); + }); return controller_interface::return_type::OK; } @@ -198,7 +204,8 @@ auto ThrusterAllocationMatrixController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { - const Eigen::Vector6d reference_wrench(reference_interfaces_.data()); + common::interfaces::read_values(ordered_exported_reference_interfaces_, reference_values_); + const Eigen::Vector6d reference_wrench(reference_values_.data()); const Eigen::VectorXd thrust(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench); for (auto && [interface, value] : std::views::zip(command_interfaces_, thrust)) { @@ -213,7 +220,7 @@ auto ThrusterAllocationMatrixController::update_and_write_commands( controller_state_.header.stamp = time; controller_state_.time_step = period.seconds(); - controller_state_.reference = reference_interfaces_; + controller_state_.reference = reference_values_; for (std::size_t i = 0; i < n_thrusters_; i++) { const auto out = command_interfaces_[i].get_optional(); diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index b75fbd3..ccd6ab2 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,11 @@ # Changelog for package thruster_controllers +## 0.5.2 (2026-11-03) + +- Exports reference interfaces with `on_export_reference_interfaces_list`, replacing the `on_export_reference_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` +- Links `controller_common`, which was previously declared but unused + ## 0.5.1 (2026-08-14) - Declares dependencies that were previously pulled in transitively diff --git a/thruster_controllers/CMakeLists.txt b/thruster_controllers/CMakeLists.txt index 1ec0a5f..438bad3 100644 --- a/thruster_controllers/CMakeLists.txt +++ b/thruster_controllers/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(control_msgs REQUIRED) +find_package(controller_common REQUIRED) find_package(ament_cmake REQUIRED) find_package(generate_parameter_library REQUIRED) find_package(pluginlib REQUIRED) @@ -57,6 +58,7 @@ target_link_libraries( realtime_tools::realtime_tools ${std_msgs_TARGETS} ${control_msgs_TARGETS} + controller_common::controller_common ) pluginlib_export_plugin_description_file(controller_interface thruster_controllers.xml) @@ -83,6 +85,7 @@ ament_export_dependencies( "realtime_tools" "std_msgs" "control_msgs" + "controller_common" ) ament_package() diff --git a/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp b/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp index cd02c9c..e9abdcb 100644 --- a/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp @@ -27,8 +27,8 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64.hpp" // auto-generated by generate_parameter_library @@ -58,7 +58,7 @@ class GazeboPassthroughController : public controller_interface::ChainableContro -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -67,7 +67,7 @@ class GazeboPassthroughController : public controller_interface::ChainableContro auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; using ControllerState = control_msgs::msg::SingleDOFStateStamped; diff --git a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp index 811b0e4..b7a4ec9 100644 --- a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp @@ -27,8 +27,8 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64.hpp" // auto-generated by generate_parameter_library @@ -56,7 +56,7 @@ class PolynomialThrustCurveController : public controller_interface::ChainableCo -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -65,7 +65,7 @@ class PolynomialThrustCurveController : public controller_interface::ChainableCo auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; using ControllerState = control_msgs::msg::SingleDOFStateStamped; diff --git a/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp b/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp index 7ff95dd..98b1ca5 100644 --- a/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp @@ -27,8 +27,8 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "std_msgs/msg/float64.hpp" // auto-generated by generate_parameter_library @@ -56,7 +56,7 @@ class RotationRateController : public controller_interface::ChainableControllerI -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -65,7 +65,7 @@ class RotationRateController : public controller_interface::ChainableControllerI auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; using ControllerState = control_msgs::msg::SingleDOFStateStamped; diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 59bb8a6..a6c5595 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.5.1 + 0.5.2 A collection of thruster controllers for AUV control Evan Palmer diff --git a/thruster_controllers/src/gz_passthrough_controller.cpp b/thruster_controllers/src/gz_passthrough_controller.cpp index fd2a0ac..1a8be5d 100644 --- a/thruster_controllers/src/gz_passthrough_controller.cpp +++ b/thruster_controllers/src/gz_passthrough_controller.cpp @@ -20,6 +20,10 @@ #include "thruster_controllers/gz_passthrough_controller.hpp" +#include +#include + +#include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace thruster_controllers @@ -52,7 +56,7 @@ auto GazeboPassthroughController::on_configure(const rclcpp_lifecycle::State & / -> controller_interface::CallbackReturn { configure_parameters(); - reference_.writeFromNonRT(std_msgs::msg::Float64()); + reference_.set(std_msgs::msg::Float64()); // publisher used to write commands to the thruster // the topic name should be the same as the one used by the gazebo thruster plugin and the @@ -61,7 +65,7 @@ auto GazeboPassthroughController::on_configure(const rclcpp_lifecycle::State & / reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); @@ -76,8 +80,10 @@ auto GazeboPassthroughController::on_configure(const rclcpp_lifecycle::State & / auto GazeboPassthroughController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - reference_.readFromNonRT()->data = std::numeric_limits::quiet_NaN(); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + std_msgs::msg::Float64 reference; + reference.data = std::numeric_limits::quiet_NaN(); + reference_.set(reference); + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -96,40 +102,36 @@ auto GazeboPassthroughController::state_interface_configuration() const -> contr return state_interface_config; } -auto GazeboPassthroughController::on_export_reference_interfaces() -> std::vector +auto GazeboPassthroughController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(1, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); - - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT), - &reference_interfaces_[0]); // NOLINT(readability-container-data-pointer) - - return interfaces; + return {common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT))}; } auto GazeboPassthroughController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - reference_interfaces_[0] = current_reference->data; - current_reference->data = std::numeric_limits::quiet_NaN(); + // consume the latest reference so that it is only applied once + reference_.try_set([this](std_msgs::msg::Float64 & reference) -> void { + std::ignore = ordered_exported_reference_interfaces_[0]->set_value(reference.data); + reference.data = std::numeric_limits::quiet_NaN(); + }); return controller_interface::return_type::OK; } auto GazeboPassthroughController::update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { - const double reference = reference_interfaces_[0]; + const double reference = + ordered_exported_reference_interfaces_[0]->get_optional().value_or(std::numeric_limits::quiet_NaN()); std_msgs::msg::Float64 msg = std_msgs::msg::Float64(); msg.data = reference; passthrough_pub_->publish(msg); // NOLINT(portability-template-virtual-member-function) controller_state_.header.stamp = time; - controller_state_.dof_state.reference = reference_interfaces_[0]; + controller_state_.dof_state.reference = reference; controller_state_.dof_state.time_step = period.seconds(); controller_state_.dof_state.output = reference; rt_controller_state_pub_->try_publish(controller_state_); diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp index 6f6b1e9..4b46e8e 100644 --- a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp +++ b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp @@ -22,8 +22,11 @@ #include #include +#include #include +#include +#include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace thruster_controllers @@ -72,12 +75,12 @@ auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State { configure_parameters(); - reference_.writeFromNonRT(std_msgs::msg::Float64()); + reference_.set(std_msgs::msg::Float64()); command_interfaces_.reserve(1); reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); @@ -92,8 +95,10 @@ auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State auto PolynomialThrustCurveController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - reference_.readFromNonRT()->data = std::numeric_limits::quiet_NaN(); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + std_msgs::msg::Float64 reference; + reference.data = std::numeric_limits::quiet_NaN(); + reference_.set(reference); + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -115,28 +120,22 @@ auto PolynomialThrustCurveController::state_interface_configuration() const return state_interface_config; } -auto PolynomialThrustCurveController::on_export_reference_interfaces() - -> std::vector +auto PolynomialThrustCurveController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(1, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); - - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT), - &reference_interfaces_[0]); // NOLINT(readability-container-data-pointer) - - return interfaces; + return {common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT))}; } auto PolynomialThrustCurveController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - reference_interfaces_[0] = current_reference->data; - current_reference->data = std::numeric_limits::quiet_NaN(); + // consume the latest reference so that it is only applied once + reference_.try_set([this](std_msgs::msg::Float64 & reference) -> void { + std::ignore = ordered_exported_reference_interfaces_[0]->set_value(reference.data); + reference.data = std::numeric_limits::quiet_NaN(); + }); return controller_interface::return_type::OK; } @@ -144,7 +143,9 @@ auto PolynomialThrustCurveController::update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { - auto reference = reference_interfaces_[0]; + const double reference_value = + ordered_exported_reference_interfaces_[0]->get_optional().value_or(std::numeric_limits::quiet_NaN()); + double reference = reference_value; int pwm = params_.neutral_pwm; if (!std::isnan(reference)) { @@ -160,7 +161,7 @@ auto PolynomialThrustCurveController::update_and_write_commands( const auto out = command_interfaces_[0].get_optional(); controller_state_.header.stamp = time; - controller_state_.dof_state.reference = reference_interfaces_[0]; + controller_state_.dof_state.reference = reference_value; controller_state_.dof_state.time_step = period.seconds(); controller_state_.dof_state.output = out.value_or(std::numeric_limits::quiet_NaN()); rt_controller_state_pub_->try_publish(controller_state_); diff --git a/thruster_controllers/src/rotation_rate_controller.cpp b/thruster_controllers/src/rotation_rate_controller.cpp index 140273d..d93e09e 100644 --- a/thruster_controllers/src/rotation_rate_controller.cpp +++ b/thruster_controllers/src/rotation_rate_controller.cpp @@ -22,7 +22,10 @@ #include #include +#include +#include +#include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace thruster_controllers @@ -72,12 +75,12 @@ auto RotationRateController::on_configure(const rclcpp_lifecycle::State & /*prev -> controller_interface::CallbackReturn { configure_parameters(); - reference_.writeFromNonRT(std_msgs::msg::Float64()); + reference_.set(std_msgs::msg::Float64()); command_interfaces_.reserve(1); reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); @@ -92,8 +95,10 @@ auto RotationRateController::on_configure(const rclcpp_lifecycle::State & /*prev auto RotationRateController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - reference_.readFromNonRT()->data = std::numeric_limits::quiet_NaN(); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + std_msgs::msg::Float64 reference; + reference.data = std::numeric_limits::quiet_NaN(); + reference_.set(reference); + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -113,34 +118,31 @@ auto RotationRateController::state_interface_configuration() const -> controller return state_interface_config; } -auto RotationRateController::on_export_reference_interfaces() -> std::vector +auto RotationRateController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(1, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); - - interfaces.emplace_back( - get_node()->get_name(), - std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT), - &reference_interfaces_[0]); // NOLINT(readability-container-data-pointer) - - return interfaces; + return {common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", thruster_name_, hardware_interface::HW_IF_EFFORT))}; } auto RotationRateController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - reference_interfaces_[0] = current_reference->data; - current_reference->data = std::numeric_limits::quiet_NaN(); + // consume the latest reference so that it is only applied once + reference_.try_set([this](std_msgs::msg::Float64 & reference) -> void { + std::ignore = ordered_exported_reference_interfaces_[0]->set_value(reference.data); + reference.data = std::numeric_limits::quiet_NaN(); + }); return controller_interface::return_type::OK; } auto RotationRateController::update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type { - double reference = reference_interfaces_[0]; + const double reference_value = + ordered_exported_reference_interfaces_[0]->get_optional().value_or(std::numeric_limits::quiet_NaN()); + double reference = reference_value; double ang_vel = 0.0; if (!std::isnan(reference)) { @@ -157,7 +159,7 @@ auto RotationRateController::update_and_write_commands(const rclcpp::Time & time const auto out = command_interfaces_[0].get_optional(); controller_state_.header.stamp = time; - controller_state_.dof_state.reference = reference_interfaces_[0]; + controller_state_.dof_state.reference = reference_value; controller_state_.dof_state.time_step = period.seconds(); controller_state_.dof_state.output = out.value_or(std::numeric_limits::quiet_NaN()); rt_controller_state_pub_->try_publish(controller_state_); diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index 2ac0aa9..634f39d 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package topic_sensors +## 0.5.2 (2026-11-03) + +- Exports the odom sensor state interfaces through `export_unlisted_state_interface_descriptions`, replacing the `export_state_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` + ## 0.5.1 (2026-08-14) - Removes the mobile-to-maritime message transform option from the odom sensor diff --git a/topic_sensors/include/topic_sensors/odom_sensor.hpp b/topic_sensors/include/topic_sensors/odom_sensor.hpp index b05b15a..d0c25ca 100644 --- a/topic_sensors/include/topic_sensors/odom_sensor.hpp +++ b/topic_sensors/include/topic_sensors/odom_sensor.hpp @@ -28,7 +28,7 @@ #include "hardware_interface/sensor_interface.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "realtime_tools/realtime_buffer.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" namespace topic_sensors { @@ -43,17 +43,19 @@ class OdomSensor : public hardware_interface::SensorInterface auto on_configure(const rclcpp_lifecycle::State & previous_state) -> hardware_interface::CallbackReturn override; - auto export_state_interfaces() -> std::vector override; + auto export_unlisted_state_interface_descriptions() -> std::vector override; auto read(const rclcpp::Time & time, const rclcpp::Duration & period) -> hardware_interface::return_type override; private: - realtime_tools::RealtimeBuffer state_; + realtime_tools::RealtimeThreadSafeBox state_; std::shared_ptr> state_sub_; std::array pose_dofs_{"x", "y", "z", "qx", "qy", "qz", "qw"}; std::array twist_dofs_{"x", "y", "z", "rx", "ry", "rz"}; - std::array state_values_; // we can't use an object with dynamic memory allocation for the state values + + // handles to the exported state interfaces, ordered as [pose_dofs_..., twist_dofs_...] + std::array state_handles_; std::shared_ptr node_; rclcpp::executors::SingleThreadedExecutor executor_; diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index ba417d9..810ff83 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.5.1 + 0.5.2 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/topic_sensors/src/odom_sensor.cpp b/topic_sensors/src/odom_sensor.cpp index b610aa8..6abfcba 100644 --- a/topic_sensors/src/odom_sensor.cpp +++ b/topic_sensors/src/odom_sensor.cpp @@ -22,6 +22,7 @@ #include #include +#include #include "controller_common/common.hpp" @@ -41,8 +42,22 @@ auto OdomSensor::on_init(const hardware_interface::HardwareComponentInterfacePar auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> hardware_interface::CallbackReturn { - common::messages::reset_message(state_.readFromNonRT()); - state_values_.fill(std::numeric_limits::quiet_NaN()); + nav_msgs::msg::Odometry initial_state; + common::messages::reset_message(&initial_state); + state_.set(initial_state); + + // the state interfaces are exported by the framework after on_init, so the handles are available here + std::size_t idx = 0; + for (const auto & dof : pose_dofs_) { + const auto prefix = prefix_.empty() ? dof : std::format("{}/{}", prefix_, dof); + state_handles_[idx++] = + get_state_interface_handle(std::format("{}/{}", prefix, hardware_interface::HW_IF_POSITION)); + } + for (const auto & dof : twist_dofs_) { + const auto prefix = prefix_.empty() ? dof : std::format("{}/{}", prefix_, dof); + state_handles_[idx++] = + get_state_interface_handle(std::format("{}/{}", prefix, hardware_interface::HW_IF_VELOCITY)); + } const std::string topic = info_.hardware_parameters.at("topic"); if (topic.empty()) { @@ -55,32 +70,30 @@ auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/ topic, rclcpp::SensorDataQoS(), [this](const std::shared_ptr msg) { // NOLINT - state_.writeFromNonRT(*msg); + state_.set(*msg); }); return hardware_interface::CallbackReturn::SUCCESS; } -// NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto OdomSensor::export_state_interfaces() -> std::vector +auto OdomSensor::export_unlisted_state_interface_descriptions() -> std::vector { - std::vector interfaces; - interfaces.reserve(pose_dofs_.size() + twist_dofs_.size()); - - for (const auto [i, dof] : std::views::enumerate(pose_dofs_)) { - interfaces.emplace_back( - prefix_.empty() ? dof : std::format("{}/{}", prefix_, dof), - hardware_interface::HW_IF_POSITION, - &state_values_[i]); - } + std::vector descriptions; + descriptions.reserve(pose_dofs_.size() + twist_dofs_.size()); - for (const auto [j, dof] : std::views::enumerate(twist_dofs_)) { - interfaces.emplace_back( - prefix_.empty() ? dof : std::format("{}/{}", prefix_, dof), - hardware_interface::HW_IF_VELOCITY, - &state_values_[pose_dofs_.size() + j]); + auto add_description = [this, &descriptions](const std::string & dof, const std::string & interface) -> void { + hardware_interface::InterfaceInfo info; + info.name = interface; + descriptions.emplace_back(prefix_.empty() ? dof : std::format("{}/{}", prefix_, dof), info); + }; + + for (const auto & dof : pose_dofs_) { + add_description(dof, hardware_interface::HW_IF_POSITION); + } + for (const auto & dof : twist_dofs_) { + add_description(dof, hardware_interface::HW_IF_VELOCITY); } - return interfaces; + return descriptions; } auto OdomSensor::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) @@ -88,8 +101,11 @@ auto OdomSensor::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /* { if (rclcpp::ok()) { executor_.spin_some(); - const auto * current_state = state_.readFromRT(); - std::ranges::copy(common::messages::to_vector(*current_state), state_values_.begin()); + state_.try_get([this](const nav_msgs::msg::Odometry & state) -> void { + for (const auto & [handle, value] : std::views::zip(state_handles_, common::messages::to_vector(state))) { + std::ignore = set_state(handle, value, false); + } + }); } return hardware_interface::return_type::OK; } diff --git a/trajectory_controllers/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md index b2baed0..39aa5cf 100644 --- a/trajectory_controllers/CHANGELOG.md +++ b/trajectory_controllers/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package trajectory_controllers +## 0.5.2 (2026-11-03) + +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` and atomics +- Detects new trajectories in the control loop, fixing a race where a trajectory received shortly after another could skip re-anchoring to the current state + ## 0.5.1 (2026-08-14) - Declares dependencies that were previously pulled in transitively diff --git a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp index 4892f2e..06c9803 100644 --- a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp @@ -20,6 +20,8 @@ #pragma once +#include +#include #include #include "auv_control_msgs/action/follow_cartesian_trajectory.hpp" @@ -27,9 +29,9 @@ #include "controller_common/common.hpp" #include "controller_interface/controller_interface.hpp" #include "rclcpp_action/server.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" #include "trajectory_controllers/cartesian_trajectory.hpp" @@ -68,6 +70,9 @@ class CartesianTrajectoryController : public controller_interface::ControllerInt [[nodiscard]] auto validate_trajectory(const auv_control_msgs::msg::CartesianTrajectory & trajectory) const -> bool; + // hand a new trajectory off to the control loop and stop holding position + auto set_trajectory(const auv_control_msgs::msg::CartesianTrajectory & trajectory) -> void; + // controller state using ControllerState = auv_control_msgs::msg::CartesianTrajectoryControllerStateStamped; std::shared_ptr> controller_state_pub_; @@ -78,7 +83,10 @@ class CartesianTrajectoryController : public controller_interface::ControllerInt // 1. using the topic interface - when available, this is preferred over the tf2 interface // 2. using the state interfaces - this is the default, but often not available // 3. using tf2 - this is the most common interface, but requires a transform to be published and is not as robust - realtime_tools::RealtimeBuffer state_; + // the latest state is written to state_ by the state subscriber (option 1) or by the control loop (options 2 & 3) + // the control loop operates on its local copy, system_state_ + realtime_tools::RealtimeThreadSafeBox state_; + geometry_msgs::msg::Pose system_state_; std::shared_ptr> state_sub_; std::unique_ptr tf_buffer_; @@ -88,22 +96,25 @@ class CartesianTrajectoryController : public controller_interface::ControllerInt // the action server is preferred and easier to integrate into state-machines/behavior trees, but can be a bit // cumbersome to interface with. on the other hand, the topic interface is easier to use, but doesn't integrate // well with high-level coordinators - realtime_tools::RealtimeBuffer rt_trajectory_; + // a new trajectory is detected in the control loop by a change in the pointer, at which point it is re-sampled from + // the current state + realtime_tools::RealtimeThreadSafeBox> rt_trajectory_; + std::shared_ptr trajectory_; // control loop copy std::shared_ptr> trajectory_sub_; using FollowTrajectory = auv_control_msgs::action::FollowCartesianTrajectory; using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; - using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + using RealtimeGoalHandleBox = realtime_tools::RealtimeThreadSafeBox; std::shared_ptr> action_server_; - RealtimeGoalHandleBuffer rt_active_goal_; - realtime_tools::RealtimeBuffer rt_goal_in_progress_; + RealtimeGoalHandleBox rt_active_goal_; + RealtimeGoalHandlePtr active_goal_; // control loop copy + std::atomic rt_goal_in_progress_{false}; std::shared_ptr goal_handle_timer_; std::chrono::nanoseconds action_monitor_period_ = std::chrono::nanoseconds(50000000); // 50ms - realtime_tools::RealtimeBuffer rt_first_sample_; // used to sample the trajectory for the first time - realtime_tools::RealtimeBuffer rt_holding_position_; // used to maintain the current pose + std::atomic rt_holding_position_{true}; // used to maintain the current pose // the update period is used to sample the "next" trajectory point rclcpp::Duration update_period_{0, 0}; @@ -115,7 +126,7 @@ class CartesianTrajectoryController : public controller_interface::ControllerInt // the default tolerances are extracted from the parameters and applied when the action interface is not used // if the action interface is being used, then the tolerances set in the goal are applied double default_path_tolerance_, default_goal_tolerance_; - realtime_tools::RealtimeBuffer rt_goal_tolerance_, rt_path_tolerance_; + std::atomic rt_goal_tolerance_{0.0}, rt_path_tolerance_{0.0}; std::vector dofs_; std::size_t n_dofs_; diff --git a/trajectory_controllers/package.xml b/trajectory_controllers/package.xml index 33c84ba..f68b79e 100644 --- a/trajectory_controllers/package.xml +++ b/trajectory_controllers/package.xml @@ -3,7 +3,7 @@ trajectory_controllers - 0.5.1 + 0.5.2 Trajectory controllers for underwater vehicles and manipulator systems Evan Palmer diff --git a/trajectory_controllers/src/cartesian_trajectory_controller.cpp b/trajectory_controllers/src/cartesian_trajectory_controller.cpp index 4e7437e..e6dc3b6 100644 --- a/trajectory_controllers/src/cartesian_trajectory_controller.cpp +++ b/trajectory_controllers/src/cartesian_trajectory_controller.cpp @@ -21,8 +21,11 @@ #include "trajectory_controllers/cartesian_trajectory_controller.hpp" #include +#include #include +#include #include +#include #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -81,8 +84,8 @@ auto CartesianTrajectoryController::configure_parameters() -> controller_interfa default_path_tolerance_ = params_.path_tolerance; default_goal_tolerance_ = params_.goal_tolerance; - rt_path_tolerance_.writeFromNonRT(params_.path_tolerance); - rt_goal_tolerance_.writeFromNonRT(params_.goal_tolerance); + rt_path_tolerance_ = params_.path_tolerance; + rt_goal_tolerance_ = params_.goal_tolerance; auto period = std::chrono::duration(1.0 / params_.action_monitor_rate); action_monitor_period_ = std::chrono::duration_cast(period); @@ -127,12 +130,22 @@ auto CartesianTrajectoryController::validate_trajectory( return true; } +auto CartesianTrajectoryController::set_trajectory(const auv_control_msgs::msg::CartesianTrajectory & trajectory) + -> void +{ + // construct the trajectory outside of the lock to minimize the time that the control loop can't access it + auto new_trajectory = std::make_shared(trajectory, state_.get()); + rt_trajectory_.set( + [&new_trajectory](std::shared_ptr & current) -> void { current = std::move(new_trajectory); }); + rt_holding_position_ = false; +} + auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { configure_parameters(); - state_.writeFromNonRT(geometry_msgs::msg::Pose()); + state_.set(geometry_msgs::msg::Pose()); command_interfaces_.reserve(n_dofs_); update_period_ = rclcpp::Duration(0.0, static_cast(1.0e9 / static_cast(get_update_rate()))); @@ -141,7 +154,7 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & "~/state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - state_.writeFromNonRT(*msg); + state_.set(*msg); }); } else { if (params_.lookup_transform) { @@ -164,11 +177,9 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & updated_msg.header.stamp = get_node()->now(); } RCLCPP_INFO(logger_, "Received new trajectory message"); // NOLINT - rt_trajectory_.writeFromNonRT(CartesianTrajectory(updated_msg, *state_.readFromNonRT())); - rt_goal_tolerance_.writeFromNonRT(default_goal_tolerance_); - rt_path_tolerance_.writeFromNonRT(default_path_tolerance_); - rt_first_sample_.writeFromNonRT(true); - rt_holding_position_.writeFromNonRT(false); + rt_goal_tolerance_ = default_goal_tolerance_; + rt_path_tolerance_ = default_path_tolerance_; + set_trajectory(updated_msg); }); auto handle_goal = [this]( @@ -188,46 +199,45 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & if (common::math::isclose(start_time.seconds(), 0.0)) { updated_msg.header.stamp = get_node()->now(); } - rt_trajectory_.writeFromNonRT(CartesianTrajectory(updated_msg, *state_.readFromNonRT())); - rt_first_sample_.writeFromNonRT(true); - rt_holding_position_.writeFromNonRT(false); + set_trajectory(updated_msg); RCLCPP_INFO(logger_, "Accepted new trajectory goal"); // NOLINT return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }; auto handle_cancel = [this](const std::shared_ptr> gh) { // NOLINT RCLCPP_INFO(logger_, "Received cancel action goal"); // NOLINT - const auto active_goal = *rt_active_goal_.readFromNonRT(); + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&active_goal](const RealtimeGoalHandlePtr & goal) -> void { active_goal = goal; }); if (active_goal && active_goal->gh_ == gh) { RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT auto action_result = std::make_shared(); active_goal->setCanceled(action_result); - rt_holding_position_.writeFromNonRT(true); - rt_first_sample_.writeFromNonRT(true); - rt_goal_in_progress_.writeFromNonRT(false); + rt_holding_position_ = true; + rt_goal_in_progress_ = false; } return rclcpp_action::CancelResponse::ACCEPT; }; auto handle_accepted = [this](std::shared_ptr> gh) { // NOLINT RCLCPP_INFO(logger_, "Received accepted action goal"); // NOLINT - rt_goal_in_progress_.writeFromNonRT(true); - const auto active_goal = *rt_active_goal_.readFromNonRT(); + rt_goal_in_progress_ = true; + RealtimeGoalHandlePtr active_goal; + rt_active_goal_.get([&active_goal](const RealtimeGoalHandlePtr & goal) -> void { active_goal = goal; }); if (active_goal) { RCLCPP_INFO(logger_, "Canceling active goal"); // NOLINT auto action_result = std::make_shared(); action_result->error_code = FollowTrajectory::Result::INVALID_GOAL; action_result->error_string = "Current goal cancelled by a new incoming action."; active_goal->setCanceled(action_result); - rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + rt_active_goal_.set(nullptr); } - rt_goal_tolerance_.writeFromNonRT(gh->get_goal()->goal_tolerance); - rt_path_tolerance_.writeFromNonRT(gh->get_goal()->path_tolerance); + rt_goal_tolerance_ = gh->get_goal()->goal_tolerance; + rt_path_tolerance_ = gh->get_goal()->path_tolerance; const RealtimeGoalHandlePtr rt_gh = std::make_shared(gh); rt_gh->execute(); - rt_active_goal_.writeFromNonRT(rt_gh); + rt_active_goal_.set([&rt_gh](RealtimeGoalHandlePtr & goal) -> void { goal = rt_gh; }); goal_handle_timer_ = nullptr; goal_handle_timer_ = @@ -247,9 +257,9 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & auto CartesianTrajectoryController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - rt_first_sample_.writeFromNonRT(true); - rt_holding_position_.writeFromNonRT(true); // hold position until a trajectory is received - common::messages::reset_message(state_.readFromNonRT()); + rt_holding_position_ = true; // hold position until a trajectory is received + common::messages::reset_message(&system_state_); + state_.set(system_state_); return controller_interface::CallbackReturn::SUCCESS; } @@ -289,7 +299,7 @@ auto CartesianTrajectoryController::state_interface_configuration() const auto CartesianTrajectoryController::update_system_state() -> controller_interface::return_type { - auto * system_state = state_.readFromRT(); + auto * system_state = &system_state_; if (params_.lookup_transform) { const auto to_frame = params_.child_frame_id; const auto from_frame = params_.frame_id; @@ -316,12 +326,22 @@ auto CartesianTrajectoryController::update_system_state() -> controller_interfac system_state->orientation.y = get_value(state_interfaces_[4]); system_state->orientation.z = get_value(state_interfaces_[5]); system_state->orientation.w = get_value(state_interfaces_[6]); + } else { + // keep the previous state if the latest message can't be accessed without blocking + if (const auto state = state_.try_get(); state.has_value()) { + *system_state = state.value(); + } } if (common::math::has_nan(common::messages::to_vector(*system_state))) { RCLCPP_DEBUG(logger_, "Received state with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } + + // share the latest state with the trajectory callbacks + if (!params_.use_external_measured_states) { + std::ignore = state_.try_set(*system_state); + } return controller_interface::return_type::OK; } @@ -333,14 +353,16 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc return controller_interface::return_type::OK; } - const auto active_goal = *rt_active_goal_.readFromRT(); - const bool goal_in_progress = *rt_goal_in_progress_.readFromRT(); + // keep the previous goal handle if the latest one can't be accessed without blocking + rt_active_goal_.try_get([this](const RealtimeGoalHandlePtr & goal) -> void { active_goal_ = goal; }); + const auto active_goal = active_goal_; + const bool goal_in_progress = rt_goal_in_progress_; if (goal_in_progress && active_goal == nullptr) { RCLCPP_DEBUG(logger_, "Goal in progress but no active goal. Ignoring update"); // NOLINT return controller_interface::return_type::OK; } - const geometry_msgs::msg::Pose system_state = *state_.readFromRT(); + const geometry_msgs::msg::Pose system_state = system_state_; geometry_msgs::msg::Pose reference_state; common::messages::reset_message(&reference_state); auto command_state = system_state; @@ -356,7 +378,21 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc }; // hold position until a new trajectory is received - if (*rt_holding_position_.readFromRT()) { + if (rt_holding_position_) { + write_command(command_interfaces_, system_state); + publish_controller_state(); + return controller_interface::return_type::OK; + } + + // get the latest trajectory. if it can't be accessed without blocking, a new trajectory is being written, so hold the + // current pose for this update and try again on the next one + bool first_sample = false; + const bool trajectory_updated = + rt_trajectory_.try_get([this, &first_sample](const std::shared_ptr & trajectory) -> void { + first_sample = trajectory != trajectory_; + trajectory_ = trajectory; + }); + if (!trajectory_updated || trajectory_ == nullptr) { write_command(command_interfaces_, system_state); publish_controller_state(); return controller_interface::return_type::OK; @@ -364,15 +400,14 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc // set the sample time rclcpp::Time sample_time = time; - if (*rt_first_sample_.readFromRT()) { - rt_trajectory_.readFromRT()->reset_initial_state(system_state); - rt_first_sample_.writeFromNonRT(false); + if (first_sample) { + trajectory_->reset_initial_state(system_state); sample_time += period; } // we use the current sample to measure errors and the future sample as the command // the future sample should be used in order to prevent the controller from lagging - const auto * trajectory = rt_trajectory_.readFromRT(); + const auto & trajectory = trajectory_; const auto sampled_reference = trajectory->sample(sample_time); const auto sampled_command = trajectory->sample(sample_time + update_period_); @@ -390,10 +425,10 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc if (sampled_command.has_value()) { command_state = sampled_command.value(); if (!std::isnan(error)) { - const double path_tolerance = *rt_path_tolerance_.readFromRT(); + const double path_tolerance = rt_path_tolerance_; if (path_tolerance > 0.0 && error > path_tolerance) { path_tolerance_exceeded = true; - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; command_state = system_state; RCLCPP_WARN(logger_, "Aborting trajectory. Error threshold exceeded during execution: %f", error); // NOLINT RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT @@ -412,7 +447,7 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc break; } - const double goal_tolerance = *rt_goal_tolerance_.readFromRT(); + const double goal_tolerance = rt_goal_tolerance_; const double goal_error = geodesic_error(end_point.value(), system_state); RCLCPP_INFO(logger_, "CartesianTrajectory sample time is after trajectory end time."); // NOLINT if (goal_tolerance > 0.0) { @@ -424,13 +459,13 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc RCLCPP_INFO(logger_, "CartesianTrajectory execution completed successfully"); // NOLINT } } - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; RCLCPP_INFO(logger_, "Holding position until a new trajectory is received"); // NOLINT } break; default: // default to position hold - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; break; } } @@ -450,19 +485,19 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; action_result->error_string = "CartesianTrajectory execution aborted. Goal tolerance exceeded."; active_goal->setAborted(action_result); - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; } else if (trajectory_suceeded) { auto action_result = std::make_shared(); action_result->error_code = FollowTrajectory::Result::SUCCESSFUL; action_result->error_string = "CartesianTrajectory execution completed successfully!"; active_goal->setSucceeded(action_result); - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; } else if (path_tolerance_exceeded) { auto action_result = std::make_shared(); action_result->error_code = FollowTrajectory::Result::PATH_TOLERANCE_VIOLATED; action_result->error_string = "CartesianTrajectory execution aborted. Path tolerance exceeded."; active_goal->setAborted(action_result); - rt_holding_position_.writeFromNonRT(true); + rt_holding_position_ = true; } } diff --git a/twist_controllers/CHANGELOG.md b/twist_controllers/CHANGELOG.md index c6d8a3d..8ca1dd2 100644 --- a/twist_controllers/CHANGELOG.md +++ b/twist_controllers/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package twist_controllers +## 0.5.2 (2026-11-03) + +- Exports reference interfaces with `on_export_reference_interfaces_list`, replacing the `on_export_reference_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` + ## 0.5.1 (2026-08-14) - Declares dependencies that were previously pulled in transitively diff --git a/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp b/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp index d0f37ac..e32c3e9 100644 --- a/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp +++ b/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp @@ -27,8 +27,8 @@ #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" @@ -57,7 +57,7 @@ class AdaptiveIntegralTerminalSlidingModeController : public controller_interfac -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -72,12 +72,15 @@ class AdaptiveIntegralTerminalSlidingModeController : public controller_interfac // provide an interface for setting reference commands from a topic // this allows us to use tools like the keyboard teleop node - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; + // local copy of the reference interface values used to compute the control command + std::vector reference_values_; + // provide an interface for setting state values from a topic // this is mostly useful when we don't want to or can't use a topic sensor - realtime_tools::RealtimeBuffer system_state_; + realtime_tools::RealtimeThreadSafeBox system_state_; std::shared_ptr> system_state_sub_; std::vector system_state_values_; @@ -88,7 +91,7 @@ class AdaptiveIntegralTerminalSlidingModeController : public controller_interfac std::shared_ptr tf_listener_; std::string vehicle_frame_id_, inertial_frame_id_; - realtime_tools::RealtimeBuffer system_rotation_; + Eigen::Quaterniond system_rotation_{Eigen::Quaterniond::Identity()}; // only accessed from the control loop using ControllerState = control_msgs::msg::MultiDOFStateStamped; std::shared_ptr> controller_state_pub_; diff --git a/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp b/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp index 2350215..25d9833 100644 --- a/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp +++ b/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp @@ -31,8 +31,8 @@ #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" @@ -60,7 +60,7 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -73,10 +73,13 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont auto update_and_validate_interfaces() -> controller_interface::return_type; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; - realtime_tools::RealtimeBuffer system_state_; + // local copy of the reference interface values used to compute the control command + std::vector reference_values_; + + realtime_tools::RealtimeThreadSafeBox system_state_; std::shared_ptr> system_state_sub_; std::vector system_state_values_; @@ -85,7 +88,7 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont std::shared_ptr tf_listener_; std::string inertial_frame_id_, vehicle_frame_id_; - realtime_tools::RealtimeBuffer system_rotation_; + Eigen::Quaterniond system_rotation_{Eigen::Quaterniond::Identity()}; // only accessed from the control loop using ControllerState = control_msgs::msg::MultiDOFStateStamped; std::shared_ptr> controller_state_pub_; diff --git a/twist_controllers/package.xml b/twist_controllers/package.xml index 88117b8..250c95c 100644 --- a/twist_controllers/package.xml +++ b/twist_controllers/package.xml @@ -3,7 +3,7 @@ twist_controllers - 0.5.1 + 0.5.2 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp index ab7d04d..a9012d8 100644 --- a/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp +++ b/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp @@ -20,7 +20,9 @@ #include "twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp" +#include #include +#include #include "controller_common/common.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -107,8 +109,8 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_configure(const rclcpp_li { configure_parameters(); - reference_.writeFromNonRT(geometry_msgs::msg::Twist()); - system_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + reference_.set(geometry_msgs::msg::Twist()); + system_state_.set(nav_msgs::msg::Odometry()); command_interfaces_.reserve(n_dofs_); system_state_values_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); @@ -121,7 +123,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_configure(const rclcpp_li "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); // use the tf interface when we aren't getting the state from a topic @@ -131,7 +133,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_configure(const rclcpp_li "~/system_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - system_state_.writeFromNonRT(*msg); + system_state_.set(*msg); }); } else { RCLCPP_INFO(logger_, "Using state interfaces and tf2 to get the system state"); // NOLINT @@ -156,11 +158,18 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_activate(const rclcpp_lif { first_update_ = true; - system_rotation_.writeFromNonRT(Eigen::Quaterniond::Identity()); - common::messages::reset_message(reference_.readFromNonRT()); - common::messages::reset_message(system_state_.readFromNonRT()); + system_rotation_ = Eigen::Quaterniond::Identity(); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + geometry_msgs::msg::Twist reference; + common::messages::reset_message(&reference); + reference_.set(reference); + + nav_msgs::msg::Odometry system_state; + common::messages::reset_message(&system_state); + system_state_.set(system_state); + + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); + reference_values_.assign(reference_values_.size(), std::numeric_limits::quiet_NaN()); system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; @@ -200,16 +209,17 @@ auto AdaptiveIntegralTerminalSlidingModeController::state_interface_configuratio } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto AdaptiveIntegralTerminalSlidingModeController::on_export_reference_interfaces() - -> std::vector +auto AdaptiveIntegralTerminalSlidingModeController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); + reference_values_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(n_dofs_); - for (const auto [i, dof] : std::views::enumerate(dofs_)) { - interfaces.emplace_back( - get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY), &reference_interfaces_[i]); + for (const auto & dof : dofs_) { + interfaces.push_back( + common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY))); } return interfaces; @@ -219,14 +229,16 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_reference_from_subscr const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - const std::vector reference = common::messages::to_vector(*current_reference); - for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { - if (!std::isnan(ref)) { - interface = ref; + // consume the latest reference so that it is only applied once + reference_.try_set([this](geometry_msgs::msg::Twist & reference) -> void { + const std::vector values = common::messages::to_vector(reference); + for (const auto & [interface, ref] : std::views::zip(ordered_exported_reference_interfaces_, values)) { + if (!std::isnan(ref)) { + std::ignore = interface->set_value(ref); + } } - } - common::messages::reset_message(current_reference); + common::messages::reset_message(&reference); + }); return controller_interface::return_type::OK; } @@ -234,9 +246,10 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_reference_from_subscr auto AdaptiveIntegralTerminalSlidingModeController::update_system_state_values() -> controller_interface::return_type { if (params_.use_external_measured_states) { - auto * current_state = system_state_.readFromNonRT(); - std::ranges::copy(common::messages::to_vector(current_state->twist.twist), system_state_values_.begin()); - tf2::fromMsg(current_state->pose.pose.orientation, *system_rotation_.readFromRT()); + system_state_.try_get([this](const nav_msgs::msg::Odometry & state) -> void { + std::ranges::copy(common::messages::to_vector(state.twist.twist), system_state_values_.begin()); + tf2::fromMsg(state.pose.pose.orientation, system_rotation_); + }); } else { std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); @@ -244,7 +257,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_system_state_values() try { const auto t = tf_buffer_->lookupTransform(params_.odom_frame_id, params_.vehicle_frame_id, tf2::TimePointZero); - tf2::fromMsg(t.transform.rotation, *system_rotation_.readFromRT()); + tf2::fromMsg(t.transform.rotation, system_rotation_); } catch (const tf2::TransformException & e) { RCLCPP_DEBUG( // NOLINT @@ -269,7 +282,8 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_validate_interfac RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT return controller_interface::return_type::ERROR; } - if (common::math::has_nan(reference_interfaces_)) { + common::interfaces::read_values(ordered_exported_reference_interfaces_, reference_values_); + if (common::math::has_nan(reference_values_)) { RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } @@ -288,7 +302,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands( configure_parameters(); // calculate the velocity error - const std::vector error_values = common::math::calculate_error(reference_interfaces_, system_state_values_); + const std::vector error_values = common::math::calculate_error(reference_values_, system_state_values_); if (common::math::all_nan(error_values)) { RCLCPP_DEBUG(logger_, "All velocity error values are NaN. Skipping control update."); // NOLINT return controller_interface::return_type::OK; @@ -311,7 +325,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands( // calculate the computed torque control // assume that the feedforward acceleration is zero const Eigen::Vector6d u1 = alpha_ * integral_error_; - const auto & rot = system_rotation_.readFromRT()->toRotationMatrix(); + const auto rot = system_rotation_.toRotationMatrix(); const Eigen::Vector6d t0 = hydrodynamics::inverse_dynamics(*model_, u1, vel, rot); // calculate the adaptive disturbance rejection control @@ -335,7 +349,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands( controller_state_.header.stamp = time; for (auto && [i, state] : std::views::enumerate(controller_state_.dof_states)) { const auto out = command_interfaces_[i].get_optional(); - state.reference = reference_interfaces_[i]; + state.reference = reference_values_[i]; state.feedback = system_state_values_[i]; state.error = error_values[i]; state.time_step = period.seconds(); diff --git a/twist_controllers/src/integral_sliding_mode_controller.cpp b/twist_controllers/src/integral_sliding_mode_controller.cpp index 0bcd988..f76d616 100644 --- a/twist_controllers/src/integral_sliding_mode_controller.cpp +++ b/twist_controllers/src/integral_sliding_mode_controller.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "controller_common/common.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -97,8 +98,8 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & { configure_parameters(); - reference_.writeFromNonRT(geometry_msgs::msg::Twist()); - system_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + reference_.set(geometry_msgs::msg::Twist()); + system_state_.set(nav_msgs::msg::Odometry()); command_interfaces_.reserve(n_dofs_); system_state_values_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); @@ -109,7 +110,7 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); // use the tf interface when we aren't getting the state from a topic @@ -119,7 +120,7 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & "~/system_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - system_state_.writeFromNonRT(*msg); + system_state_.set(*msg); }); } else { tf_buffer_ = std::make_unique(get_node()->get_clock()); @@ -145,11 +146,18 @@ auto IntegralSlidingModeController::on_activate(const rclcpp_lifecycle::State & total_error_ = Eigen::Vector6d::Zero(); init_error_ = Eigen::Vector6d::Zero(); - system_rotation_.writeFromNonRT(Eigen::Quaterniond::Identity()); - common::messages::reset_message(reference_.readFromNonRT()); - common::messages::reset_message(system_state_.readFromNonRT()); + system_rotation_ = Eigen::Quaterniond::Identity(); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + geometry_msgs::msg::Twist reference; + common::messages::reset_message(&reference); + reference_.set(reference); + + nav_msgs::msg::Odometry system_state; + common::messages::reset_message(&system_state); + system_state_.set(system_state); + + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); + reference_values_.assign(reference_values_.size(), std::numeric_limits::quiet_NaN()); system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; @@ -189,16 +197,17 @@ auto IntegralSlidingModeController::state_interface_configuration() const } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto IntegralSlidingModeController::on_export_reference_interfaces() - -> std::vector +auto IntegralSlidingModeController::on_export_reference_interfaces_list() + -> std::vector { - reference_interfaces_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); - std::vector interfaces; - interfaces.reserve(reference_interfaces_.size()); + reference_values_.resize(n_dofs_, std::numeric_limits::quiet_NaN()); + std::vector interfaces; + interfaces.reserve(n_dofs_); - for (const auto [i, dof] : std::views::enumerate(dofs_)) { - interfaces.emplace_back( - get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY), &reference_interfaces_[i]); + for (const auto & dof : dofs_) { + interfaces.push_back( + common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_VELOCITY))); } return interfaces; @@ -208,14 +217,16 @@ auto IntegralSlidingModeController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -> controller_interface::return_type { - auto * current_reference = reference_.readFromNonRT(); - const std::vector reference = common::messages::to_vector(*current_reference); - for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { - if (!std::isnan(ref)) { - interface = ref; + // consume the latest reference so that it is only applied once + reference_.try_set([this](geometry_msgs::msg::Twist & reference) -> void { + const std::vector values = common::messages::to_vector(reference); + for (const auto & [interface, ref] : std::views::zip(ordered_exported_reference_interfaces_, values)) { + if (!std::isnan(ref)) { + std::ignore = interface->set_value(ref); + } } - } - common::messages::reset_message(current_reference); + common::messages::reset_message(&reference); + }); return controller_interface::return_type::OK; } @@ -223,9 +234,10 @@ auto IntegralSlidingModeController::update_reference_from_subscribers( auto IntegralSlidingModeController::update_system_state_values() -> controller_interface::return_type { if (params_.use_external_measured_states) { - auto * current_state = system_state_.readFromNonRT(); - std::ranges::copy(common::messages::to_vector(current_state->twist.twist), system_state_values_.begin()); - tf2::fromMsg(current_state->pose.pose.orientation, *system_rotation_.readFromRT()); + system_state_.try_get([this](const nav_msgs::msg::Odometry & state) -> void { + std::ranges::copy(common::messages::to_vector(state.twist.twist), system_state_values_.begin()); + tf2::fromMsg(state.pose.pose.orientation, system_rotation_); + }); } else { std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); @@ -233,7 +245,7 @@ auto IntegralSlidingModeController::update_system_state_values() -> controller_i try { const auto t = tf_buffer_->lookupTransform(params_.odom_frame_id, params_.vehicle_frame_id, tf2::TimePointZero); - tf2::fromMsg(t.transform.rotation, *system_rotation_.readFromRT()); + tf2::fromMsg(t.transform.rotation, system_rotation_); } catch (const tf2::TransformException & e) { RCLCPP_DEBUG( // NOLINT @@ -257,7 +269,8 @@ auto IntegralSlidingModeController::update_and_validate_interfaces() -> controll RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT return controller_interface::return_type::ERROR; } - if (common::math::has_nan(reference_interfaces_)) { + common::interfaces::read_values(ordered_exported_reference_interfaces_, reference_values_); + if (common::math::has_nan(reference_values_)) { RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } @@ -276,7 +289,7 @@ auto IntegralSlidingModeController::update_and_write_commands( configure_parameters(); // Calculate the velocity error - const std::vector error_values = common::math::calculate_error(reference_interfaces_, system_state_values_); + const std::vector error_values = common::math::calculate_error(reference_values_, system_state_values_); if (common::math::all_nan(error_values)) { RCLCPP_DEBUG(logger_, "All velocity error values are NaN. Skipping control update."); // NOLINT return controller_interface::return_type::OK; @@ -295,7 +308,7 @@ auto IntegralSlidingModeController::update_and_write_commands( // calculate the computed torque control // assume that the feedforward acceleration is zero const Eigen::Vector6d u = kp_ * error; - const auto & rot = system_rotation_.readFromRT()->toRotationMatrix(); + const auto rot = system_rotation_.toRotationMatrix(); const Eigen::Vector6d tau0 = hydrodynamics::inverse_dynamics(*model_, u, vel, rot); // calculate the disturbance rejection torque @@ -315,7 +328,7 @@ auto IntegralSlidingModeController::update_and_write_commands( controller_state_.header.stamp = time; for (auto && [i, state] : std::views::enumerate(controller_state_.dof_states)) { const auto out = command_interfaces_[i].get_optional(); - state.reference = reference_interfaces_[i]; + state.reference = reference_values_[i]; state.feedback = system_state_values_[i]; state.error = error_values[i]; state.time_step = period.seconds(); diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 4f63de6..25cb7c4 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package whole_body_controllers +## 0.5.2 (2026-11-03) + +- Exports reference interfaces with `on_export_reference_interfaces_list`, replacing the `on_export_reference_interfaces` API removed upstream +- Replaces the deprecated `RealtimeBuffer` with `RealtimeThreadSafeBox` + ## 0.5.1 (2026-08-14) - Removes the mobile-to-maritime message transform from the IK controller diff --git a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp index eb7e9e2..420ebdf 100644 --- a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp +++ b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp @@ -26,8 +26,8 @@ #include "ik_solvers/solver.hpp" #include "nav_msgs/msg/odometry.hpp" #include "pluginlib/class_loader.hpp" -#include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" +#include "realtime_tools/realtime_thread_safe_box.hpp" // auto-generated by generate_parameter_library #include @@ -56,7 +56,7 @@ class IKController : public controller_interface::ChainableControllerInterface -> controller_interface::return_type override; protected: - auto on_export_reference_interfaces() -> std::vector override; + auto on_export_reference_interfaces_list() -> std::vector override; auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) -> controller_interface::return_type override; @@ -74,13 +74,16 @@ class IKController : public controller_interface::ChainableControllerInterface std::shared_ptr model_; std::shared_ptr data_; - realtime_tools::RealtimeBuffer reference_; + realtime_tools::RealtimeThreadSafeBox reference_; std::shared_ptr> reference_sub_; + // local copy of the reference interface values used to compute the control command + std::vector reference_values_; + // allow users to send the vehicle state as a message - this can simplify integration with state estimators // that publish the vehicle state without the manipulator states std::shared_ptr> vehicle_state_sub_; - realtime_tools::RealtimeBuffer vehicle_state_; + realtime_tools::RealtimeThreadSafeBox vehicle_state_; std::vector position_state_values_, velocity_state_values_; diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index eeb2fe6..a980195 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.5.1 + 0.5.2 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp index 229272a..3f73b99 100644 --- a/whole_body_controllers/src/ik_controller.cpp +++ b/whole_body_controllers/src/ik_controller.cpp @@ -22,8 +22,10 @@ #include #include +#include #include #include +#include #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -153,8 +155,8 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state { configure_parameters(); - reference_.writeFromNonRT(geometry_msgs::msg::Pose()); - vehicle_state_.writeFromNonRT(nav_msgs::msg::Odometry()); + reference_.set(geometry_msgs::msg::Pose()); + vehicle_state_.set(nav_msgs::msg::Odometry()); command_interfaces_.reserve(n_command_interfaces_); position_state_values_.resize(position_interface_names_.size(), std::numeric_limits::quiet_NaN()); @@ -162,7 +164,7 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(*msg); + reference_.set(*msg); }); if (params_.use_external_measured_vehicle_states) { @@ -171,7 +173,7 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state "~/vehicle_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - vehicle_state_.writeFromNonRT(*msg); + vehicle_state_.set(*msg); }); } @@ -194,9 +196,16 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state auto IKController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - common::messages::reset_message(reference_.readFromNonRT()); - common::messages::reset_message(vehicle_state_.readFromNonRT()); - reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); + geometry_msgs::msg::Pose reference; + common::messages::reset_message(&reference); + reference_.set(reference); + + nav_msgs::msg::Odometry vehicle_state; + common::messages::reset_message(&vehicle_state); + vehicle_state_.set(vehicle_state); + + common::interfaces::fill(ordered_exported_reference_interfaces_, std::numeric_limits::quiet_NaN()); + reference_values_.assign(reference_values_.size(), std::numeric_limits::quiet_NaN()); position_state_values_.assign(position_state_values_.size(), std::numeric_limits::quiet_NaN()); velocity_state_values_.assign(velocity_state_values_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; @@ -274,15 +283,16 @@ auto IKController::state_interface_configuration() const -> controller_interface } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto IKController::on_export_reference_interfaces() -> std::vector +auto IKController::on_export_reference_interfaces_list() -> std::vector { - reference_interfaces_.resize(free_flyer_pos_dofs_.size(), std::numeric_limits::quiet_NaN()); - std::vector interfaces; + reference_values_.resize(free_flyer_pos_dofs_.size(), std::numeric_limits::quiet_NaN()); + std::vector interfaces; interfaces.reserve(free_flyer_pos_dofs_.size()); - for (const auto & [i, dof] : std::views::enumerate(free_flyer_pos_dofs_)) { - const std::string interface = std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION); - interfaces.emplace_back(get_node()->get_name(), interface, &reference_interfaces_[i]); + for (const auto & dof : free_flyer_pos_dofs_) { + interfaces.push_back( + common::interfaces::make_reference_interface( + get_node()->get_name(), std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION))); } return interfaces; @@ -292,12 +302,14 @@ auto IKController::on_export_reference_interfaces() -> std::vector controller_interface::return_type { - auto * current_reference = reference_.readFromRT(); - std::vector reference = common::messages::to_vector(*current_reference); - for (auto && [interface, ref] : std::views::zip(reference_interfaces_, reference)) { - interface = ref; - } - common::messages::reset_message(current_reference); + // consume the latest reference so that it is only applied once + reference_.try_set([this](geometry_msgs::msg::Pose & reference) -> void { + const std::vector values = common::messages::to_vector(reference); + for (const auto & [interface, ref] : std::views::zip(ordered_exported_reference_interfaces_, values)) { + std::ignore = interface->set_value(ref); + } + common::messages::reset_message(&reference); + }); return controller_interface::return_type::OK; } @@ -308,10 +320,11 @@ auto IKController::update_system_state_values() -> controller_interface::return_ // appropriate frame, so we can just copy the values into the system state values. otherwise, we need to transform // the states first, then save them. if (params_.use_external_measured_vehicle_states) { - const auto * state_msg = vehicle_state_.readFromRT(); - const std::vector state = common::messages::to_vector(*state_msg); - std::ranges::copy(state.begin(), state.begin() + free_flyer_pos_dofs_.size(), position_state_values_.begin()); - std::ranges::copy(state.begin() + free_flyer_pos_dofs_.size(), state.end(), velocity_state_values_.begin()); + vehicle_state_.try_get([this](const nav_msgs::msg::Odometry & state_msg) -> void { + const std::vector state = common::messages::to_vector(state_msg); + std::ranges::copy(state.begin(), state.begin() + free_flyer_pos_dofs_.size(), position_state_values_.begin()); + std::ranges::copy(state.begin() + free_flyer_pos_dofs_.size(), state.end(), velocity_state_values_.begin()); + }); } else { auto save_states = [](const auto & interfaces, auto out) -> auto { std::ranges::transform(interfaces, out, [](const auto & interface) -> auto { @@ -391,8 +404,8 @@ auto IKController::update_chained_reference_values() -> controller_interface::re // because the command interfaces expect the commands to be in the maritime coordinate frame standard, we need // this extra method to transform the values into a frame suitable for pinocchio geometry_msgs::msg::Pose reference_transformed; - common::messages::to_msg(reference_interfaces_, &reference_transformed); - std::ranges::copy(common::messages::to_vector(reference_transformed), reference_interfaces_.begin()); + common::messages::to_msg(reference_values_, &reference_transformed); + std::ranges::copy(common::messages::to_vector(reference_transformed), reference_values_.begin()); return controller_interface::return_type::OK; } @@ -402,10 +415,11 @@ auto IKController::update_and_validate_interfaces() -> controller_interface::ret RCLCPP_DEBUG(logger_, "Failed to update system state values"); // NOLINT return controller_interface::return_type::ERROR; } + common::interfaces::read_values(ordered_exported_reference_interfaces_, reference_values_); if (is_in_chained_mode()) { update_chained_reference_values(); } - if (common::math::has_nan(reference_interfaces_)) { + if (common::math::has_nan(reference_values_)) { RCLCPP_DEBUG(logger_, "Received reference with NaN value."); // NOLINT return controller_interface::return_type::ERROR; } @@ -424,7 +438,7 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons configure_parameters(); const Eigen::VectorXd q = Eigen::VectorXd::Map(position_state_values_.data(), position_state_values_.size()); - const Eigen::Isometry3d goal = to_eigen(reference_interfaces_); + const Eigen::Isometry3d goal = to_eigen(reference_values_); // TODO(anyone): add solver support for velocity states // right now we only use the positions for the solver @@ -471,7 +485,7 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons controller_state_.header.stamp = get_node()->now(); controller_state_.time_step = period.seconds(); - common::messages::to_msg(reference_interfaces_, &controller_state_.reference); + common::messages::to_msg(reference_values_, &controller_state_.reference); controller_state_.solution = point; rt_controller_state_pub_->try_publish(controller_state_); From c96f668d2eb4002604d7896beadc4ccd4f144fe2 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 3 Nov 2026 16:05:23 -0800 Subject: [PATCH 2/2] Fix missing dependencies in meta-package --- auv_controllers/CHANGELOG.md | 2 ++ auv_controllers/package.xml | 3 +++ 2 files changed, 5 insertions(+) diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index 1b221a9..7122b63 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -2,6 +2,8 @@ ## 0.5.2 (2026-11-03) +- Adds the missing `controller_coordinator`, `impedance_controller`, and `trajectory_controllers` dependencies + ## 0.5.1 (2026-08-14) ## 0.5.0 (2026-05-29) diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 20929d6..64886b8 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -18,10 +18,13 @@ auv_control_msgs controller_common + controller_coordinator ik_solvers + impedance_controller thruster_allocation_matrix_controller thruster_controllers topic_sensors + trajectory_controllers twist_controllers whole_body_controllers