Merge remote-tracking branch 'origin/main' into jazzy

evan-palmer · evan-palmer · commit 9b82490cc716 · 2025-07-22T16:11:39.000-07:00
diff --git a/README.md b/README.md
@@ -40,6 +40,20 @@ To learn more about how to use the controllers provided in this project, please
 refer to the [examples package](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/auv_control_demos).
 You can also find integration tutorials in the [Blue documentation](https://robotic-decision-making-lab.github.io/blue).
 
+## Citation
+
+If you find this code to be useful in your research, please consider citing our
+relevant paper:
+
+```
+@inproceedings{palmer2024angler,
+  author={Palmer, Evan and Holm, Christopher and Hollinger, Geoffrey},
+  title={{Angler: An Autonomy Framework for Intervention Tasks with Lightweight Underwater Vehicle Manipulator Systems}},
+  booktitle={IEEE International Conference on Robotics and Automation},
+  year={2024}
+}
+```
+
 ## Getting help
 
 If you have questions regarding usage of auv_controllers or regarding
diff --git a/auv_control_demos/CHANGELOG.md b/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)
diff --git a/auv_control_demos/package.xml b/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="mitchcol@oregonstate.edu">Colin Mitchell</maintainer>
diff --git a/auv_control_msgs/CHANGELOG.md b/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)
diff --git a/auv_control_msgs/package.xml b/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="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
diff --git a/auv_controllers/CHANGELOG.md b/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)
diff --git a/auv_controllers/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/controller_common/CHANGELOG.md b/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)
diff --git a/controller_common/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/controller_coordinator/CHANGELOG.md b/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.
diff --git a/controller_coordinator/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/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)
diff --git a/end_effector_trajectory_controller/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp b/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
diff --git a/end_effector_trajectory_controller/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/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
diff --git a/ik_solvers/CHANGELOG.md b/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)
diff --git a/ik_solvers/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/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)
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
@@ -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_;
diff --git a/thruster_allocation_matrix_controller/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp b/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;
 }
 
diff --git a/thruster_controllers/CHANGELOG.md b/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)
diff --git a/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp b/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_;
diff --git a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp b/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_;
diff --git a/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp b/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_;
diff --git a/thruster_controllers/package.xml b/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="evanp922@gmail.com">Evan Palmer</maintainer>
diff --git a/thruster_controllers/src/gz_passthrough_controller.cpp b/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;
 }
diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp b/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;
 }
diff --git a/thruster_controllers/src/rotation_rate_controller.cpp b/thruster_controllers/src/rotation_rate_controller.cpp
diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md
diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml
diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md
diff --git a/velocity_controllers/include/velocity_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp b/velocity_controllers/include/velocity_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp
diff --git a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp
diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml
diff --git a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp
diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp
diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md
diff --git a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp
diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml
diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp
