Skip to content

Commit

Permalink
Updated ISMC to use twist stamped state message and refactored realti…
Browse files Browse the repository at this point in the history
…me buffers to copy message instead of pointer (#45) (#48)

(cherry picked from commit 0a29970)

Co-authored-by: Evan Palmer <evanp922@gmail.com>
  • Loading branch information
mergify[bot] and evan-palmer authored Sep 25, 2024
1 parent adaa8aa commit c9c58a1
Show file tree
Hide file tree
Showing 6 changed files with 52 additions and 52 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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>(
Expand All @@ -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;
}
Expand Down Expand Up @@ -211,20 +210,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;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,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_;
Expand Down
11 changes: 5 additions & 6 deletions thruster_controllers/src/polynomial_thrust_curve_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_ =
Expand All @@ -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;
}
Expand Down Expand Up @@ -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<double>::quiet_NaN();
reference_interfaces_[0] = current_reference->data;
current_reference->data = std::numeric_limits<double>::quiet_NaN();

return controller_interface::return_type::OK;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -84,11 +84,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.
Expand Down
58 changes: 30 additions & 28 deletions velocity_controllers/src/integral_sliding_mode_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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;
}

Expand Down Expand Up @@ -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);

Expand All @@ -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);
});
}

Expand Down Expand Up @@ -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());
Expand All @@ -259,20 +262,20 @@ auto IntegralSlidingModeController::update_reference_from_subscribers(
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;
}
Expand All @@ -283,20 +286,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();
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand Down

0 comments on commit c9c58a1

Please sign in to comment.