Updated ISMC to use twist stamped state message and refactored realti…

…me buffers to copy message instead of pointer (#45) (#47)

(cherry picked from commit 0a29970)

Co-authored-by: Evan Palmer <evanp922@gmail.com>

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<std::shared_ptr<geometry_msgs::msg::Wrench>> reference_;
+  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_;

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<geometry_msgs::msg::Wrench> wrench_msg) -> void  // NOLINT
+auto reset_wrench_msg(geometry_msgs::msg::Wrench * wrench_msg) -> void  // NOLINT
 {
   wrench_msg->force.x = std::numeric_limits<double>::quiet_NaN();
   wrench_msg->force.y = std::numeric_limits<double>::quiet_NaN();
@@ -122,16 +122,15 @@ auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::St
     return ret;
   }
 
-  const auto reference_msg = std::make_shared<geometry_msgs::msg::Wrench>();
-  reference_.writeFromNonRT(reference_msg);
+  reference_.writeFromNonRT(geometry_msgs::msg::Wrench());
 
   command_interfaces_.reserve(num_thrusters_);
 
   reference_sub_ = get_node()->create_subscription<geometry_msgs::msg::Wrench>(
     "~/reference",
     rclcpp::SystemDefaultsQoS(),
     [this](const std::shared_ptr<geometry_msgs::msg::Wrench> msg) {  // NOLINT
-      reference_.writeFromNonRT(msg);
+      reference_.writeFromNonRT(*msg);
     });
 
   controller_state_pub_ = get_node()->create_publisher<auv_control_msgs::msg::MultiActuatorStateStamped>(
@@ -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<double>::quiet_NaN());
   return controller_interface::CallbackReturn::SUCCESS;
 }
@@ -209,20 +208,20 @@ auto ThrusterAllocationMatrixController::update_reference_from_subscribers() ->
   auto * current_reference = reference_.readFromNonRT();
 
   const std::vector<double> 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])) {
       reference_interfaces_[i] = wrench[i];
     }
   }
 
-  reset_wrench_msg(*current_reference);
+  reset_wrench_msg(current_reference);
 
   return controller_interface::return_type::OK;
 }

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<std::shared_ptr<std_msgs::msg::Float64>> reference_;
+  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_;

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<std_msgs::msg::Float64>();
-  reference_.writeFromNonRT(reference_msg);
+  reference_.writeFromNonRT(std_msgs::msg::Float64());
 
   command_interfaces_.reserve(1);
 
   reference_sub_ = get_node()->create_subscription<std_msgs::msg::Float64>(
     "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<std_msgs::msg::Float64> 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<double>::quiet_NaN();
+  reference_.readFromNonRT()->data = std::numeric_limits<double>::quiet_NaN();
   reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits<double>::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<double>::quiet_NaN();
+  reference_interfaces_[0] = current_reference->data;
+  current_reference->data = std::numeric_limits<double>::quiet_NaN();
 
   return controller_interface::return_type::OK;
 }

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<std::shared_ptr<geometry_msgs::msg::Twist>> reference_;
+  realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist> reference_;
   std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Twist>> reference_sub_;
 
-  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Twist>> system_state_;
-  std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::Twist>> system_state_sub_;
+  realtime_tools::RealtimeBuffer<geometry_msgs::msg::Twist> system_state_;
+  std::shared_ptr<rclcpp::Subscription<geometry_msgs::msg::TwistStamped>> system_state_sub_;
   std::vector<double> system_state_values_;
 
   // We need the system rotation from the inertial frame to the vehicle frame for the hydrodynamic model.

velocity_controllers/src/integral_sliding_mode_controller.cpp

@@ -39,7 +39,7 @@ namespace velocity_controllers
 namespace
 {
 
-void reset_twist_msg(std::shared_ptr<geometry_msgs::msg::Twist> msg)  // NOLINT
+void reset_twist_msg(geometry_msgs::msg::Twist * msg)  // NOLINT
 {
   msg->linear.x = std::numeric_limits<double>::quiet_NaN();
   msg->linear.y = std::numeric_limits<double>::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<geometry_msgs::msg::Twist>();
-  reference_.writeFromNonRT(reference_msg);
-
-  const auto system_state_msg = std::make_shared<geometry_msgs::msg::Twist>();
-  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<geometry_msgs::msg::Twist> 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<geometry_msgs::msg::Twist>(
+    system_state_sub_ = get_node()->create_subscription<geometry_msgs::msg::TwistStamped>(
       "~/system_state",
       rclcpp::SystemDefaultsQoS(),
-      [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) {  // NOLINT
-        system_state_.writeFromNonRT(msg);
+      [this](const std::shared_ptr<geometry_msgs::msg::TwistStamped> 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<double>::quiet_NaN());
   system_state_values_.assign(system_state_values_.size(), std::numeric_limits<double>::quiet_NaN());
@@ -257,20 +260,20 @@ auto IntegralSlidingModeController::update_reference_from_subscribers() -> contr
   auto * current_reference = reference_.readFromNonRT();
 
   const std::vector<double> 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])) {
       reference_interfaces_[i] = reference[i];
     }
   }
 
-  reset_twist_msg(*current_reference);
+  reset_twist_msg(current_reference);
 
   return controller_interface::return_type::OK;
 }
@@ -281,20 +284,20 @@ auto IntegralSlidingModeController::update_system_state_values() -> controller_i
     auto * current_system_state = system_state_.readFromRT();
 
     const std::vector<double> 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])) {
         system_state_values_[i] = state[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