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 38b9fc1..f52a615 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 @@ -75,7 +75,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 32a1143..b4c94b6 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; } @@ -211,12 +210,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])) { @@ -224,7 +223,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 e20dedd..1ed11da 100644 --- a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp @@ -71,7 +71,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 7d32b60..152db5f 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; } @@ -155,8 +154,8 @@ auto PolynomialThrustCurveController::update_reference_from_subscribers( 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(); + 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 9045fc2..c281aee 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" @@ -84,11 +84,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 f0f02b6..8381172 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()); @@ -259,12 +262,12 @@ auto IntegralSlidingModeController::update_reference_from_subscribers( 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])) { @@ -272,7 +275,7 @@ auto IntegralSlidingModeController::update_reference_from_subscribers( } } - reset_twist_msg(*current_reference); + reset_twist_msg(current_reference); return controller_interface::return_type::OK; } @@ -283,12 +286,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])) { @@ -296,7 +299,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(); @@ -334,7 +337,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; } @@ -379,7 +382,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