Remove deprecated ros2_control API (backport #69)

auv_control_demos/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package auv_control_demos
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

auv_control_demos/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_demos</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="[email protected]">Colin Mitchell</maintainer>

auv_control_msgs/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package auv_control_msgs
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

auv_control_msgs/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_control_msgs</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="[email protected]">Rakesh Vivekanandan</maintainer>

auv_controllers/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package auv_controllers
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

auv_controllers/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_controllers</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

controller_common/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package controller_common
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

controller_common/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_common</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Common interfaces for controllers used in this project</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

controller_coordinator/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package controller_coordinator
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 - Addresses a lifetime bug in the coordinator service responses.

controller_coordinator/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>controller_coordinator</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>A high-level node used to load and activate/deactivate control systems</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

end_effector_trajectory_controller/CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog for package controller_common
 
+## 0.3.2 (2025-07-22)
+
+- Replaces the deprecated `unlockAndPublish` API with `try_publish`
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp

@@ -74,6 +74,7 @@ class EndEffectorTrajectoryController : public controller_interface::ControllerI
   using ControllerState = auv_control_msgs::msg::EndEffectorTrajectoryControllerState;
   std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
+  ControllerState controller_state_;
 
   // the end effector states can be captured in one of three ways:
   // 1. using the topic interface - when available, this is preferred over the tf2 interface

end_effector_trajectory_controller/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>end_effector_trajectory_controller</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>End effector trajectory tracking controller for UVMS control</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp

@@ -338,14 +338,12 @@ auto EndEffectorTrajectoryController::update(const rclcpp::Time & time, const rc
   double error = std::numeric_limits<double>::quiet_NaN();
 
   auto publish_controller_state = [this, &reference_state, &end_effector_state, &error, &command_state]() {
-    if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
-      rt_controller_state_pub_->msg_.header.stamp = get_node()->now();
-      rt_controller_state_pub_->msg_.reference = reference_state;
-      rt_controller_state_pub_->msg_.feedback = end_effector_state;
-      rt_controller_state_pub_->msg_.error = error;
-      rt_controller_state_pub_->msg_.output = command_state;
-      rt_controller_state_pub_->unlockAndPublish();
-    }
+    controller_state_.header.stamp = get_node()->now();
+    controller_state_.reference = reference_state;
+    controller_state_.feedback = end_effector_state;
+    controller_state_.error = error;
+    controller_state_.output = command_state;
+    rt_controller_state_pub_->try_publish(controller_state_);
   };
 
   // hold position until a new trajectory is received

ik_solvers/CHANGELOG.md

@@ -1,5 +1,7 @@
 # Changelog for package ik_solvers
 
+## 0.3.2 (2025-07-22)
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

ik_solvers/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>ik_solvers</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Inverse kinematics solvers used for whole-body control</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

thruster_allocation_matrix_controller/CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog for package thruster_allocation_matrix_controller
 
+## 0.3.2 (2025-07-22)
+
+- Replaces the deprecated `unlockAndPublish` API with `try_publish`
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp

@@ -76,9 +76,10 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl
   realtime_tools::RealtimeBuffer<geometry_msgs::msg::Wrench> reference_;
   std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Wrench>> reference_sub_;
 
-  std::shared_ptr<rclcpp::Publisher<auv_control_msgs::msg::MultiActuatorStateStamped>> controller_state_pub_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<auv_control_msgs::msg::MultiActuatorStateStamped>>
-    rt_controller_state_pub_;
+  using ControllerState = auv_control_msgs::msg::MultiActuatorStateStamped;
+  std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
+  ControllerState controller_state_;
 
   std::shared_ptr<thruster_allocation_matrix_controller::ParamListener> param_listener_;
   thruster_allocation_matrix_controller::Params params_;

thruster_allocation_matrix_controller/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_allocation_matrix_controller</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>Thruster allocation matrix controller used to convert wrench commands into thrust commands</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp

@@ -134,19 +134,15 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St
       reference_.writeFromNonRT(*msg);
     });
 
-  controller_state_pub_ = get_node()->create_publisher<auv_control_msgs::msg::MultiActuatorStateStamped>(
-    "~/status", rclcpp::SystemDefaultsQoS());
+  controller_state_pub_ = get_node()->create_publisher<ControllerState>("~/status", rclcpp::SystemDefaultsQoS());
 
   rt_controller_state_pub_ =
-    std::make_unique<realtime_tools::RealtimePublisher<auv_control_msgs::msg::MultiActuatorStateStamped>>(
-      controller_state_pub_);
+    std::make_unique<realtime_tools::RealtimePublisher<ControllerState>>(controller_state_pub_);
 
-  rt_controller_state_pub_->lock();
-  rt_controller_state_pub_->msg_.output_names = thruster_names_;
-  rt_controller_state_pub_->msg_.reference_names.assign(dofs_.begin(), dofs_.end());
-  rt_controller_state_pub_->msg_.reference.resize(n_dofs_, std::numeric_limits<double>::quiet_NaN());
-  rt_controller_state_pub_->msg_.output.resize(n_thrusters_, std::numeric_limits<double>::quiet_NaN());
-  rt_controller_state_pub_->unlock();
+  controller_state_.output_names = thruster_names_;
+  controller_state_.reference_names.assign(dofs_.begin(), dofs_.end());
+  controller_state_.reference.resize(n_dofs_, std::numeric_limits<double>::quiet_NaN());
+  controller_state_.output.resize(n_thrusters_, std::numeric_limits<double>::quiet_NaN());
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -257,19 +253,17 @@ auto ThrusterAllocationMatrixController::update_and_write_commands(
     }
   }
 
-  if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
-    rt_controller_state_pub_->msg_.header.stamp = time;
-    rt_controller_state_pub_->msg_.time_step = period.seconds();
-    rt_controller_state_pub_->msg_.reference = reference_interfaces_;
+  controller_state_.header.stamp = time;
+  controller_state_.time_step = period.seconds();
+  controller_state_.reference = reference_interfaces_;
 
-    for (std::size_t i = 0; i < n_thrusters_; i++) {
-      const auto out = command_interfaces_[i].get_optional();
-      rt_controller_state_pub_->msg_.output[i] = out.value_or(std::numeric_limits<double>::quiet_NaN());
-    }
-
-    rt_controller_state_pub_->unlockAndPublish();
+  for (std::size_t i = 0; i < n_thrusters_; i++) {
+    const auto out = command_interfaces_[i].get_optional();
+    controller_state_.output[i] = out.value_or(std::numeric_limits<double>::quiet_NaN());
   }
 
+  rt_controller_state_pub_->try_publish(controller_state_);
+
   return controller_interface::return_type::OK;
 }
 

thruster_controllers/CHANGELOG.md

@@ -1,5 +1,9 @@
 # Changelog for package thruster_controllers
 
+## 0.3.2 (2025-07-22)
+
+- Replaces the deprecated `unlockAndPublish` API with `try_publish`
+
 ## 0.3.1 (2025-07-09)
 
 ## 0.3.0 (2025-06-07)

thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp

@@ -73,8 +73,10 @@ class GazeboPassthroughController : public controller_interface::ChainableContro
   realtime_tools::RealtimeBuffer<std_msgs::msg::Float64> reference_;
   std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64>> reference_sub_;
 
-  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::SingleDOFStateStamped>> controller_state_pub_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::SingleDOFStateStamped>> rt_controller_state_pub_;
+  using ControllerState = control_msgs::msg::SingleDOFStateStamped;
+  std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
+  ControllerState controller_state_;
 
   std::shared_ptr<gz_passthrough_controller::ParamListener> param_listener_;
   gz_passthrough_controller::Params params_;

thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp

@@ -71,8 +71,10 @@ class PolynomialThrustCurveController : public controller_interface::ChainableCo
   realtime_tools::RealtimeBuffer<std_msgs::msg::Float64> reference_;
   std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64>> reference_sub_;
 
-  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::SingleDOFStateStamped>> controller_state_pub_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::SingleDOFStateStamped>> rt_controller_state_pub_;
+  using ControllerState = control_msgs::msg::SingleDOFStateStamped;
+  std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
+  ControllerState controller_state_;
 
   std::shared_ptr<polynomial_thrust_curve_controller::ParamListener> param_listener_;
   polynomial_thrust_curve_controller::Params params_;

thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp

@@ -71,8 +71,10 @@ class RotationRateController : public controller_interface::ChainableControllerI
   realtime_tools::RealtimeBuffer<std_msgs::msg::Float64> reference_;
   std::shared_ptr<rclcpp::Subscription<std_msgs::msg::Float64>> reference_sub_;
 
-  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::SingleDOFStateStamped>> controller_state_pub_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::SingleDOFStateStamped>> rt_controller_state_pub_;
+  using ControllerState = control_msgs::msg::SingleDOFStateStamped;
+  std::shared_ptr<rclcpp::Publisher<ControllerState>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<ControllerState>> rt_controller_state_pub_;
+  ControllerState controller_state_;
 
   std::shared_ptr<rotation_rate_controller::ParamListener> param_listener_;
   rotation_rate_controller::Params params_;

thruster_controllers/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>thruster_controllers</name>
-  <version>0.3.1</version>
+  <version>0.3.2</version>
   <description>A collection of thruster controllers for AUV control</description>
 
   <maintainer email="[email protected]">Evan Palmer</maintainer>

thruster_controllers/src/gz_passthrough_controller.cpp

@@ -69,16 +69,11 @@ auto GazeboPassthroughController::on_configure(const rclcpp_lifecycle::State & /
       reference_.writeFromNonRT(*msg);
     });
 
-  controller_state_pub_ =
-    get_node()->create_publisher<control_msgs::msg::SingleDOFStateStamped>("~/status", rclcpp::SystemDefaultsQoS());
-
+  controller_state_pub_ = get_node()->create_publisher<ControllerState>("~/status", rclcpp::SystemDefaultsQoS());
   rt_controller_state_pub_ =
-    std::make_unique<realtime_tools::RealtimePublisher<control_msgs::msg::SingleDOFStateStamped>>(
-      controller_state_pub_);
+    std::make_unique<realtime_tools::RealtimePublisher<ControllerState>>(controller_state_pub_);
 
-  rt_controller_state_pub_->lock();
-  rt_controller_state_pub_->msg_.dof_state.name = thruster_name_;
-  rt_controller_state_pub_->unlock();
+  controller_state_.dof_state.name = thruster_name_;
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -138,13 +133,11 @@ auto GazeboPassthroughController::update_and_write_commands(const rclcpp::Time &
   msg.data = reference;
   passthrough_pub_->publish(msg);
 
-  if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
-    rt_controller_state_pub_->msg_.header.stamp = time;
-    rt_controller_state_pub_->msg_.dof_state.reference = reference_interfaces_[0];
-    rt_controller_state_pub_->msg_.dof_state.time_step = period.seconds();
-    rt_controller_state_pub_->msg_.dof_state.output = reference;
-    rt_controller_state_pub_->unlockAndPublish();
-  }
+  controller_state_.header.stamp = time;
+  controller_state_.dof_state.reference = reference_interfaces_[0];
+  controller_state_.dof_state.time_step = period.seconds();
+  controller_state_.dof_state.output = reference;
+  rt_controller_state_pub_->try_publish(controller_state_);
 
   return controller_interface::return_type::OK;
 }

thruster_controllers/src/polynomial_thrust_curve_controller.cpp

@@ -90,16 +90,11 @@ auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State
       reference_.writeFromNonRT(*msg);
     });
 
-  controller_state_pub_ =
-    get_node()->create_publisher<control_msgs::msg::SingleDOFStateStamped>("~/status", rclcpp::SystemDefaultsQoS());
-
+  controller_state_pub_ = get_node()->create_publisher<ControllerState>("~/status", rclcpp::SystemDefaultsQoS());
   rt_controller_state_pub_ =
-    std::make_unique<realtime_tools::RealtimePublisher<control_msgs::msg::SingleDOFStateStamped>>(
-      controller_state_pub_);
+    std::make_unique<realtime_tools::RealtimePublisher<ControllerState>>(controller_state_pub_);
 
-  rt_controller_state_pub_->lock();
-  rt_controller_state_pub_->msg_.dof_state.name = thruster_name_;
-  rt_controller_state_pub_->unlock();
+  controller_state_.dof_state.name = thruster_name_;
 
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -176,14 +171,12 @@ auto PolynomialThrustCurveController::update_and_write_commands(
     RCLCPP_WARN(logger_, "Failed to set command for thruster %s", thruster_name_.c_str());  // NOLINT
   }
 
-  if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
-    const auto out = command_interfaces_[0].get_optional();
-    rt_controller_state_pub_->msg_.header.stamp = time;
-    rt_controller_state_pub_->msg_.dof_state.reference = reference_interfaces_[0];
-    rt_controller_state_pub_->msg_.dof_state.time_step = period.seconds();
-    rt_controller_state_pub_->msg_.dof_state.output = out.value_or(std::numeric_limits<double>::quiet_NaN());
-    rt_controller_state_pub_->unlockAndPublish();
-  }
+  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.time_step = period.seconds();
+  controller_state_.dof_state.output = out.value_or(std::numeric_limits<double>::quiet_NaN());
+  rt_controller_state_pub_->try_publish(controller_state_);
 
   return controller_interface::return_type::OK;
 }