From 792416dd02671552273fb8da09ffc504bad9055b Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 24 Sep 2024 23:48:34 -0700 Subject: [PATCH] Updated ISMC to use twist stamped state message and refactored realtime buffers to copy message instead of pointer (#45) (cherry picked from commit 0a29970c1e1d3679bc318ed7a0cc6d599dfd9b81) --- .../thruster_allocation_matrix_controller.hpp | 2 +- .../thruster_allocation_matrix_controller.cpp | 23 ++++---- .../polynomial_thrust_curve_controller.hpp | 2 +- .../polynomial_thrust_curve_controller.cpp | 11 ++-- .../integral_sliding_mode_controller.hpp | 8 +-- .../src/integral_sliding_mode_controller.cpp | 58 ++++++++++--------- 6 files changed, 52 insertions(+), 52 deletions(-) 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 267e6eb..073add0 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 @@ -74,7 +74,7 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer> reference_; + realtime_tools::RealtimeBuffer reference_; std::shared_ptr> reference_sub_; std::shared_ptr> controller_state_pub_; 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 5ed6389..8549954 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -31,7 +31,7 @@ namespace thruster_allocation_matrix_controller namespace { -auto reset_wrench_msg(std::shared_ptr wrench_msg) -> void // NOLINT +auto reset_wrench_msg(geometry_msgs::msg::Wrench * wrench_msg) -> void // NOLINT { wrench_msg->force.x = std::numeric_limits::quiet_NaN(); wrench_msg->force.y = std::numeric_limits::quiet_NaN(); @@ -122,8 +122,7 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St return ret; } - const auto reference_msg = std::make_shared(); - reference_.writeFromNonRT(reference_msg); + reference_.writeFromNonRT(geometry_msgs::msg::Wrench()); command_interfaces_.reserve(num_thrusters_); @@ -131,7 +130,7 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(msg); + reference_.writeFromNonRT(*msg); }); controller_state_pub_ = get_node()->create_publisher( @@ -153,7 +152,7 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St auto ThrusterAllocationMatrixController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) -> controller_interface::CallbackReturn { - reset_wrench_msg(*(reference_.readFromNonRT())); + reset_wrench_msg(reference_.readFromNonRT()); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -209,12 +208,12 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers() -> auto * current_reference = reference_.readFromNonRT(); const std::vector wrench = { - (*current_reference)->force.x, - (*current_reference)->force.y, - (*current_reference)->force.z, - (*current_reference)->torque.x, - (*current_reference)->torque.y, - (*current_reference)->torque.z}; + current_reference->force.x, + current_reference->force.y, + current_reference->force.z, + current_reference->torque.x, + current_reference->torque.y, + current_reference->torque.z}; for (std::size_t i = 0; i < wrench.size(); ++i) { if (!std::isnan(wrench[i])) { @@ -222,7 +221,7 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers() -> } } - reset_wrench_msg(*current_reference); + reset_wrench_msg(current_reference); return controller_interface::return_type::OK; } 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 0c9923c..d7fd54c 100644 --- a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp @@ -70,7 +70,7 @@ class PolynomialThrustCurveController : public controller_interface::ChainableCo auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer> reference_; + realtime_tools::RealtimeBuffer reference_; std::shared_ptr> reference_sub_; std::shared_ptr> controller_state_pub_; diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp index a18c51d..d40ae40 100644 --- a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp +++ b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp @@ -81,14 +81,13 @@ auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State return ret; } - const auto reference_msg = std::make_shared(); - reference_.writeFromNonRT(reference_msg); + reference_.writeFromNonRT(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_.writeFromNonRT(*msg); }); controller_state_pub_ = @@ -107,7 +106,7 @@ 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_.readFromNonRT()->data = std::numeric_limits::quiet_NaN(); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } @@ -153,8 +152,8 @@ auto PolynomialThrustCurveController::on_export_reference_interfaces() auto PolynomialThrustCurveController::update_reference_from_subscribers() -> controller_interface::return_type { auto * current_reference = reference_.readFromNonRT(); - reference_interfaces_[0] = (*current_reference)->data; - (*current_reference)->data = std::numeric_limits::quiet_NaN(); + reference_interfaces_[0] = current_reference->data; + current_reference->data = std::numeric_limits::quiet_NaN(); return controller_interface::return_type::OK; } 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 9edd7d0..34d9705 100644 --- a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp @@ -30,7 +30,7 @@ #include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "hydrodynamics/eigen.hpp" #include "hydrodynamics/hydrodynamics.hpp" #include "rclcpp/rclcpp.hpp" @@ -83,11 +83,11 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont auto configure_parameters() -> controller_interface::CallbackReturn; - realtime_tools::RealtimeBuffer> reference_; + realtime_tools::RealtimeBuffer reference_; std::shared_ptr> reference_sub_; - realtime_tools::RealtimeBuffer> system_state_; - std::shared_ptr> system_state_sub_; + realtime_tools::RealtimeBuffer system_state_; + std::shared_ptr> system_state_sub_; std::vector system_state_values_; // We need the system rotation from the inertial frame to the vehicle frame for the hydrodynamic model. diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 367f7ec..1e09d07 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,7 +39,7 @@ namespace velocity_controllers namespace { -void reset_twist_msg(std::shared_ptr msg) // NOLINT +void reset_twist_msg(geometry_msgs::msg::Twist * msg) // NOLINT { msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); @@ -62,6 +62,12 @@ auto IntegralSlidingModeController::on_init() -> controller_interface::CallbackR return controller_interface::CallbackReturn::ERROR; } + // Notify users about this. This can be confusing for folks that expect the controller to work without a reference + // or state message. + RCLCPP_INFO( + get_node()->get_logger(), + "Reference and state messages are required for operation - commands will not be sent until both are received."); + return controller_interface::CallbackReturn::SUCCESS; } @@ -177,11 +183,8 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & return ret; } - const auto reference_msg = std::make_shared(); - reference_.writeFromNonRT(reference_msg); - - const auto system_state_msg = std::make_shared(); - system_state_.writeFromNonRT(system_state_msg); + reference_.writeFromNonRT(geometry_msgs::msg::Twist()); + system_state_.writeFromNonRT(geometry_msgs::msg::Twist()); command_interfaces_.reserve(DOF); @@ -191,16 +194,16 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - reference_.writeFromNonRT(msg); + reference_.writeFromNonRT(*msg); }); // NOLINT // If we aren't reading from the state interfaces, subscribe to the system state topic if (params_.use_external_measured_states) { - system_state_sub_ = get_node()->create_subscription( + system_state_sub_ = get_node()->create_subscription( "~/system_state", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { // NOLINT - system_state_.writeFromNonRT(msg); + [this](const std::shared_ptr msg) { // NOLINT + system_state_.writeFromNonRT(msg->twist); }); } @@ -235,8 +238,8 @@ auto IntegralSlidingModeController::on_activate(const rclcpp_lifecycle::State & system_rotation_.writeFromNonRT(Eigen::Quaterniond::Identity()); - reset_twist_msg(*(reference_.readFromNonRT())); - reset_twist_msg(*(system_state_.readFromNonRT())); + reset_twist_msg(reference_.readFromNonRT()); + reset_twist_msg(system_state_.readFromNonRT()); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); @@ -257,12 +260,12 @@ auto IntegralSlidingModeController::update_reference_from_subscribers() -> contr auto * current_reference = reference_.readFromNonRT(); const std::vector reference = { - (*current_reference)->linear.x, - (*current_reference)->linear.y, - (*current_reference)->linear.z, - (*current_reference)->angular.x, - (*current_reference)->angular.y, - (*current_reference)->angular.z}; + current_reference->linear.x, + current_reference->linear.y, + current_reference->linear.z, + current_reference->angular.x, + current_reference->angular.y, + current_reference->angular.z}; for (std::size_t i = 0; i < reference.size(); ++i) { if (!std::isnan(reference[i])) { @@ -270,7 +273,7 @@ auto IntegralSlidingModeController::update_reference_from_subscribers() -> contr } } - reset_twist_msg(*current_reference); + reset_twist_msg(current_reference); return controller_interface::return_type::OK; } @@ -281,12 +284,12 @@ auto IntegralSlidingModeController::update_system_state_values() -> controller_i auto * current_system_state = system_state_.readFromRT(); const std::vector state = { - (*current_system_state)->linear.x, - (*current_system_state)->linear.y, - (*current_system_state)->linear.z, - (*current_system_state)->angular.x, - (*current_system_state)->angular.y, - (*current_system_state)->angular.z}; + current_system_state->linear.x, + current_system_state->linear.y, + current_system_state->linear.z, + current_system_state->angular.x, + current_system_state->angular.y, + current_system_state->angular.z}; for (std::size_t i = 0; i < state.size(); ++i) { if (!std::isnan(state[i])) { @@ -294,7 +297,7 @@ auto IntegralSlidingModeController::update_system_state_values() -> controller_i } } - reset_twist_msg(*current_system_state); + reset_twist_msg(current_system_state); } else { for (std::size_t i = 0; i < system_state_values_.size(); ++i) { system_state_values_[i] = state_interfaces_[i].get_value(); @@ -332,7 +335,7 @@ auto IntegralSlidingModeController::update_and_write_commands( auto all_nan = std ::all_of(velocity_error_values.begin(), velocity_error_values.end(), [&](double i) { return std::isnan(i); }); if (all_nan) { - RCLCPP_DEBUG(get_node()->get_logger(), "All reference and system state values are NaN. Skipping control update."); + RCLCPP_DEBUG(get_node()->get_logger(), "All velocity error values are NaN. Skipping control update."); return controller_interface::return_type::OK; } @@ -377,7 +380,6 @@ auto IntegralSlidingModeController::update_and_write_commands( // Apply the sign function to the surface // Right now, we use the tanh function to reduce the chattering effect. - // TODO(evan-palmer): Implement the super twisting algorithm to improve this further surface = surface.unaryExpr([this](double x) { return std::tanh(x / boundary_thickness_); }); // Calculate the disturbance rejection torque