diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 462fe6a..7876c3d 100644 --- 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 index 37a8f0b..329e540 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.3.1 + 0.3.2 Example package that includes demos for using auv_controllers in individual and chained modes Colin Mitchell diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index 82b0c13..21d5616 100644 --- 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 index 08ec095..674044f 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -3,7 +3,7 @@ auv_control_msgs - 0.3.1 + 0.3.2 Custom messages for AUV controllers Rakesh Vivekanandan diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index dc2bde4..f305f3e 100644 --- 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 index 35e271f..27a3d1a 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.3.1 + 0.3.2 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index 39e78d6..995af80 100644 --- 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 index 8ec6a16..5528922 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.3.1 + 0.3.2 Common interfaces for controllers used in this project Evan Palmer diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md index 1ded739..e7823f1 100644 --- 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 index 1417390..8199a1c 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -3,7 +3,7 @@ controller_coordinator - 0.3.1 + 0.3.2 A high-level node used to load and activate/deactivate control systems Evan Palmer diff --git a/end_effector_trajectory_controller/CHANGELOG.md b/end_effector_trajectory_controller/CHANGELOG.md index 16286f2..43a5783 100644 --- 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 index 8b513fe..1227848 100644 --- 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> controller_state_pub_; std::unique_ptr> 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 index 1d3faf7..6a40245 100644 --- a/end_effector_trajectory_controller/package.xml +++ b/end_effector_trajectory_controller/package.xml @@ -3,7 +3,7 @@ end_effector_trajectory_controller - 0.3.1 + 0.3.2 End effector trajectory tracking controller for UVMS control Evan Palmer diff --git a/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp b/end_effector_trajectory_controller/src/end_effector_trajectory_controller.cpp index 7af5d84..8008d7d 100644 --- 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::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 index f34ec21..c5c35bc 100644 --- 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 index 7bc05de..c2f653f 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.3.1 + 0.3.2 Inverse kinematics solvers used for whole-body control Evan Palmer diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index bad6526..33cb1c3 100644 --- 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 index 975e791..a8c0aa8 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 @@ -76,9 +76,10 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl realtime_tools::RealtimeBuffer reference_; std::shared_ptr> reference_sub_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> - rt_controller_state_pub_; + using ControllerState = auv_control_msgs::msg::MultiActuatorStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr param_listener_; thruster_allocation_matrix_controller::Params params_; diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index e0b2525..231f27a 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.3.1 + 0.3.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 75297a4..9872729 100644 --- 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( - "~/status", rclcpp::SystemDefaultsQoS()); + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>( - controller_state_pub_); + std::make_unique>(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::quiet_NaN()); - rt_controller_state_pub_->msg_.output.resize(n_thrusters_, std::numeric_limits::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::quiet_NaN()); + controller_state_.output.resize(n_thrusters_, std::numeric_limits::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::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::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 index afaf585..97942e3 100644 --- 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 index f2c7cff..bc81140 100644 --- 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 reference_; std::shared_ptr> reference_sub_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; + using ControllerState = control_msgs::msg::SingleDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr 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 index 22dc97d..35f879b 100644 --- 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 reference_; std::shared_ptr> reference_sub_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; + using ControllerState = control_msgs::msg::SingleDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr 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 index 08dca8d..d082e89 100644 --- 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 reference_; std::shared_ptr> reference_sub_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; + using ControllerState = control_msgs::msg::SingleDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr param_listener_; rotation_rate_controller::Params params_; diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index 37ea7e6..bb9022d 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.3.1 + 0.3.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 0db4578..e9a4292 100644 --- 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("~/status", rclcpp::SystemDefaultsQoS()); - + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>( - controller_state_pub_); + std::make_unique>(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 index 3b75f47..937170e 100644 --- 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("~/status", rclcpp::SystemDefaultsQoS()); - + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>( - controller_state_pub_); + std::make_unique>(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::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::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 index d994283..d6207fa 100644 --- a/thruster_controllers/src/rotation_rate_controller.cpp +++ b/thruster_controllers/src/rotation_rate_controller.cpp @@ -82,16 +82,11 @@ auto RotationRateController::on_configure(const rclcpp_lifecycle::State & /*prev reference_.writeFromNonRT(*msg); }); - controller_state_pub_ = - get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); - + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>( - controller_state_pub_); + std::make_unique>(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; } @@ -162,14 +157,12 @@ auto RotationRateController::update_and_write_commands(const rclcpp::Time & time return controller_interface::return_type::ERROR; } - 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::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::quiet_NaN()); + rt_controller_state_pub_->try_publish(controller_state_); return controller_interface::return_type::OK; } diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index d6855d4..a33abf2 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package topic_sensors +## 0.3.2 (2025-07-22) + ## 0.3.1 (2025-07-09) ## 0.3.0 (2025-06-07) diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index a172c74..4109c28 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.3.1 + 0.3.2 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer diff --git a/velocity_controllers/CHANGELOG.md b/velocity_controllers/CHANGELOG.md index 68f96e8..831408f 100644 --- a/velocity_controllers/CHANGELOG.md +++ b/velocity_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package velocity_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/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 index 407e70c..964096f 100644 --- 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 @@ -95,8 +95,10 @@ class AdaptiveIntegralTerminalSlidingModeController : public controller_interfac std::string vehicle_frame_id_, inertial_frame_id_; realtime_tools::RealtimeBuffer system_rotation_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; + using ControllerState = control_msgs::msg::MultiDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr param_listener_; adaptive_integral_terminal_sliding_mode_controller::Params params_; diff --git a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp index bd07989..4ef5028 100644 --- a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp @@ -105,8 +105,10 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont std::string inertial_frame_id_, vehicle_frame_id_; realtime_tools::RealtimeBuffer system_rotation_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> rt_controller_state_pub_; + using ControllerState = control_msgs::msg::MultiDOFStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; std::shared_ptr param_listener_; integral_sliding_mode_controller::Params params_; diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 8d6ddfb..4fecfac 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -3,7 +3,7 @@ velocity_controllers - 0.3.1 + 0.3.2 A collection of velocity controllers for underwater vehicles Evan Palmer diff --git a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp index d87efc6..eae46e7 100644 --- a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp +++ b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp @@ -138,17 +138,14 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_configure(const rclcpp_li tf_listener_ = std::make_shared(*tf_buffer_); } - controller_state_pub_ = - get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>(controller_state_pub_); + std::make_unique>(controller_state_pub_); - rt_controller_state_pub_->lock(); - rt_controller_state_pub_->msg_.dof_states.resize(n_dofs_); - for (auto && [state, dof] : std::views::zip(rt_controller_state_pub_->msg_.dof_states, dofs_)) { + controller_state_.dof_states.resize(n_dofs_); + for (auto && [state, dof] : std::views::zip(controller_state_.dof_states, dofs_)) { state.name = dof; } - rt_controller_state_pub_->unlock(); return controller_interface::CallbackReturn::SUCCESS; } @@ -334,18 +331,16 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands( k1_(i, i) = val > k1_min_(i) ? k_theta_(i) * sign(s(i) - mu_(i), lambda_) : k1_min_(i); } - if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { - rt_controller_state_pub_->msg_.header.stamp = time; - for (auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) { - const auto out = command_interfaces_[i].get_optional(); - state.reference = reference_interfaces_[i]; - state.feedback = system_state_values_[i]; - state.error = error_values[i]; - state.time_step = period.seconds(); - state.output = out.value_or(std::numeric_limits::quiet_NaN()); - } - rt_controller_state_pub_->unlockAndPublish(); + 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.feedback = system_state_values_[i]; + state.error = error_values[i]; + state.time_step = period.seconds(); + state.output = out.value_or(std::numeric_limits::quiet_NaN()); } + rt_controller_state_pub_->try_publish(controller_state_); return controller_interface::return_type::OK; } diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 6d88428..43d22a4 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -135,17 +135,14 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & tf_listener_ = std::make_shared(*tf_buffer_); } - controller_state_pub_ = - get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>(controller_state_pub_); + std::make_unique>(controller_state_pub_); - rt_controller_state_pub_->lock(); - rt_controller_state_pub_->msg_.dof_states.resize(n_dofs_); - for (auto && [state, dof] : std::views::zip(rt_controller_state_pub_->msg_.dof_states, dofs_)) { + controller_state_.dof_states.resize(n_dofs_); + for (auto && [state, dof] : std::views::zip(controller_state_.dof_states, dofs_)) { state.name = dof; } - rt_controller_state_pub_->unlock(); return controller_interface::CallbackReturn::SUCCESS; } @@ -324,18 +321,16 @@ auto IntegralSlidingModeController::update_and_write_commands( } } - if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { - rt_controller_state_pub_->msg_.header.stamp = time; - for (auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) { - const auto out = command_interfaces_[i].get_optional(); - state.reference = reference_interfaces_[i]; - state.feedback = system_state_values_[i]; - state.error = error_values[i]; - state.time_step = period.seconds(); - state.output = out.value_or(std::numeric_limits::quiet_NaN()); - } - rt_controller_state_pub_->unlockAndPublish(); + 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.feedback = system_state_values_[i]; + state.error = error_values[i]; + state.time_step = period.seconds(); + state.output = out.value_or(std::numeric_limits::quiet_NaN()); } + rt_controller_state_pub_->try_publish(controller_state_); return controller_interface::return_type::OK; } diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 3144a17..57e5742 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package whole_body_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/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp index ff249fc..5a30857 100644 --- a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp +++ b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp @@ -98,9 +98,10 @@ class IKController : public controller_interface::ChainableControllerInterface std::unique_ptr param_listener_; ik_controller::Params params_; - std::shared_ptr> controller_state_pub_; - std::unique_ptr> - rt_controller_state_pub_; + using ControllerState = auv_control_msgs::msg::IKControllerStateStamped; + std::shared_ptr> controller_state_pub_; + std::unique_ptr> rt_controller_state_pub_; + ControllerState controller_state_; // store the names of the base joints std::vector free_flyer_pos_dofs_, free_flyer_vel_dofs_; diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index 536f69b..fd60c6c 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.3.1 + 0.3.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 f1b0a05..22b6bc1 100644 --- a/whole_body_controllers/src/ik_controller.cpp +++ b/whole_body_controllers/src/ik_controller.cpp @@ -182,17 +182,13 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state solver_->initialize(get_node(), model_, data_, params_.ik_solver); RCLCPP_INFO(logger_, "Configured the IK controller with solver %s", params_.ik_solver.c_str()); // NOLINT - controller_state_pub_ = get_node()->create_publisher( - "~/status", rclcpp::SystemDefaultsQoS()); + controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); rt_controller_state_pub_ = - std::make_unique>( - controller_state_pub_); + std::make_unique>(controller_state_pub_); - rt_controller_state_pub_->lock(); - rt_controller_state_pub_->msg_.solver_name = params_.ik_solver; - std::ranges::copy(position_interface_names_, std::back_inserter(rt_controller_state_pub_->msg_.position_joint_names)); - std::ranges::copy(velocity_interface_names_, std::back_inserter(rt_controller_state_pub_->msg_.velocity_joint_names)); - rt_controller_state_pub_->unlock(); + controller_state_.solver_name = params_.ik_solver; + std::ranges::copy(position_interface_names_, std::back_inserter(controller_state_.position_joint_names)); + std::ranges::copy(velocity_interface_names_, std::back_inserter(controller_state_.velocity_joint_names)); return controller_interface::CallbackReturn::SUCCESS; } @@ -474,13 +470,11 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons } } - 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_.time_step = period.seconds(); - common::messages::to_msg(reference_interfaces_, &rt_controller_state_pub_->msg_.reference); - rt_controller_state_pub_->msg_.solution = point; - rt_controller_state_pub_->unlockAndPublish(); - } + controller_state_.header.stamp = get_node()->now(); + controller_state_.time_step = period.seconds(); + common::messages::to_msg(reference_interfaces_, &controller_state_.reference); + controller_state_.solution = point; + rt_controller_state_pub_->try_publish(controller_state_); return controller_interface::return_type::OK; }