From 9490ff287f9c0b4f0c73c1b7dc03e85ac4c6ffc1 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 22 Aug 2024 11:25:50 -0700 Subject: [PATCH 1/6] Attempt to address segfault in ISMC. --- velocity_controllers/src/integral_sliding_mode_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 412ee2c..8cf6154 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,8 +39,9 @@ namespace velocity_controllers namespace { -void reset_twist_msg(std::shared_ptr msg) // NOLINT +void reset_twist_msg(const std::shared_ptr & m) // NOLINT { + auto msg(m); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); msg->linear.z = std::numeric_limits::quiet_NaN(); From f519e46ebbc961571f415c21cdb08da836cd58be Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 22 Aug 2024 11:53:55 -0700 Subject: [PATCH 2/6] Create shared_ptr earlier in process. --- .../src/integral_sliding_mode_controller.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 8cf6154..5562235 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -41,6 +41,7 @@ namespace void reset_twist_msg(const std::shared_ptr & m) // NOLINT { + // Make allocation of new shared_ptr explicit auto msg(m); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); @@ -245,11 +246,12 @@ bool IntegralSlidingModeController::on_set_chained_mode(bool /*chained_mode*/) { controller_interface::return_type IntegralSlidingModeController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - auto * current_reference = reference_.readFromNonRT(); + // Explicitly create a new shared_ptr to ensure usage it incremented + 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}; + 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}; for (size_t i = 0; i < reference.size(); ++i) { if (!std::isnan(reference[i])) { @@ -257,7 +259,7 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc } } - reset_twist_msg(*current_reference); + reset_twist_msg(current_reference); return controller_interface::return_type::OK; } @@ -265,11 +267,12 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc controller_interface::return_type IntegralSlidingModeController::update_system_state_values() { if (params_.use_external_measured_states) { - auto * current_system_state = system_state_.readFromRT(); + // Explicitly create a new shared_ptr to ensure usage it incremented + 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}; + 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}; for (size_t i = 0; i < state.size(); ++i) { if (!std::isnan(state[i])) { @@ -277,7 +280,7 @@ controller_interface::return_type IntegralSlidingModeController::update_system_s } } - reset_twist_msg(*current_system_state); + reset_twist_msg(current_system_state); } else { for (size_t i = 0; i < system_state_values_.size(); ++i) { system_state_values_[i] = state_interfaces_[i].get_value(); From 139d58eb8314853eb7f046d404915e0924a8027d Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 22 Aug 2024 12:44:43 -0700 Subject: [PATCH 3/6] Use pass by value in reset_twist_msg to ensure shared_ptr is incremented. --- velocity_controllers/src/integral_sliding_mode_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 5562235..859d558 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,10 +39,9 @@ namespace velocity_controllers namespace { -void reset_twist_msg(const std::shared_ptr & m) // NOLINT +void reset_twist_msg(std::shared_ptr msg) // NOLINT { // Make allocation of new shared_ptr explicit - auto msg(m); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); msg->linear.z = std::numeric_limits::quiet_NaN(); From 29765210fe5cad2e257130fb94a450b7d067f218 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 22 Aug 2024 12:48:05 -0700 Subject: [PATCH 4/6] Revert "Use pass by value in reset_twist_msg to ensure shared_ptr is incremented." This reverts commit 139d58eb8314853eb7f046d404915e0924a8027d. --- velocity_controllers/src/integral_sliding_mode_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 859d558..5562235 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,9 +39,10 @@ namespace velocity_controllers namespace { -void reset_twist_msg(std::shared_ptr msg) // NOLINT +void reset_twist_msg(const std::shared_ptr & m) // NOLINT { // Make allocation of new shared_ptr explicit + auto msg(m); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); msg->linear.z = std::numeric_limits::quiet_NaN(); From 5d403d3782b6f36ec4645229fa3e0d6a4688bf2f Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 22 Aug 2024 14:06:55 -0700 Subject: [PATCH 5/6] Pass shared_ptr into reset_twist_msg() by value --- velocity_controllers/src/integral_sliding_mode_controller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 5562235..a3202fd 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,10 +39,8 @@ namespace velocity_controllers namespace { -void reset_twist_msg(const std::shared_ptr & m) // NOLINT +void reset_twist_msg(std::shared_ptr msg) // NOLINT { - // Make allocation of new shared_ptr explicit - auto msg(m); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); msg->linear.z = std::numeric_limits::quiet_NaN(); From a7af79f5e4ada48330b1f9b4b42a6ca444db97b3 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Tue, 27 Aug 2024 10:11:15 -0700 Subject: [PATCH 6/6] Expliclty pass empty Twist message to realtime buffer. --- .../src/integral_sliding_mode_controller.cpp | 44 ++++++++++++------- 1 file changed, 29 insertions(+), 15 deletions(-) diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index a3202fd..d9a898e 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -39,14 +39,17 @@ namespace velocity_controllers namespace { -void reset_twist_msg(std::shared_ptr msg) // NOLINT +std::shared_ptr empty_twist_msg() // NOLINT { + auto msg = std::make_shared(); msg->linear.x = std::numeric_limits::quiet_NaN(); msg->linear.y = std::numeric_limits::quiet_NaN(); msg->linear.z = std::numeric_limits::quiet_NaN(); msg->angular.x = std::numeric_limits::quiet_NaN(); msg->angular.y = std::numeric_limits::quiet_NaN(); msg->angular.z = std::numeric_limits::quiet_NaN(); + + return msg; } } // namespace @@ -224,8 +227,8 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_activate( system_rotation_.writeFromNonRT(Eigen::Quaterniond::Identity()); - reset_twist_msg(*(reference_.readFromNonRT())); - reset_twist_msg(*(system_state_.readFromNonRT())); + reference_.writeFromNonRT(empty_twist_msg()); + system_state_.writeFromNonRT(empty_twist_msg()); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); system_state_values_.assign(system_state_values_.size(), std::numeric_limits::quiet_NaN()); @@ -244,12 +247,17 @@ bool IntegralSlidingModeController::on_set_chained_mode(bool /*chained_mode*/) { controller_interface::return_type IntegralSlidingModeController::update_reference_from_subscribers( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // Explicitly create a new shared_ptr to ensure usage it incremented - auto current_reference(*(reference_.readFromNonRT())); + // Explicitly create a new shared_ptr to ensure usage is incremented + auto current_reference = reference_.readFromNonRT(); + + // no command received yet + if (!current_reference || !(*current_reference)) { + return controller_interface::return_type::OK; + } - 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}; + 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}; for (size_t i = 0; i < reference.size(); ++i) { if (!std::isnan(reference[i])) { @@ -257,7 +265,7 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc } } - reset_twist_msg(current_reference); + reference_.writeFromNonRT(empty_twist_msg()); return controller_interface::return_type::OK; } @@ -265,12 +273,17 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc controller_interface::return_type IntegralSlidingModeController::update_system_state_values() { if (params_.use_external_measured_states) { - // Explicitly create a new shared_ptr to ensure usage it incremented - auto current_system_state(*(system_state_.readFromRT())); + // Explicitly create a new shared_ptr to ensure usage is incremented + 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}; + // no command received yet + if (!current_system_state || !(*current_system_state)) { + return controller_interface::return_type::OK; + } + + 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}; for (size_t i = 0; i < state.size(); ++i) { if (!std::isnan(state[i])) { @@ -278,7 +291,8 @@ controller_interface::return_type IntegralSlidingModeController::update_system_s } } - reset_twist_msg(current_system_state); + system_state_.writeFromNonRT(empty_twist_msg()); + } else { for (size_t i = 0; i < system_state_values_.size(); ++i) { system_state_values_[i] = state_interfaces_[i].get_value();