Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[Bugfix] Fix Segfault in ISMC #36

Draft
wants to merge 6 commits into
base: main
Choose a base branch
from
Draft

[Bugfix] Fix Segfault in ISMC #36

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9490ff2
Attempt to address segfault in ISMC.
amarburg Aug 22, 2024
f519e46
Create shared_ptr earlier in process.
amarburg Aug 22, 2024
139d58e
Use pass by value in reset_twist_msg to ensure shared_ptr is incremen…
amarburg Aug 22, 2024
2976521
Revert "Use pass by value in reset_twist_msg to ensure shared_ptr is …
amarburg Aug 22, 2024
5d403d3
Pass shared_ptr<Twist> into reset_twist_msg() by value
amarburg Aug 22, 2024
a7af79f
Expliclty pass empty Twist message to realtime buffer.
amarburg Aug 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 23 additions & 7 deletions velocity_controllers/src/integral_sliding_mode_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,14 +39,17 @@ namespace velocity_controllers
namespace
{

void reset_twist_msg(std::shared_ptr<geometry_msgs::msg::Twist> msg) // NOLINT
std::shared_ptr<geometry_msgs::msg::Twist> empty_twist_msg() // NOLINT
{
auto msg = std::make_shared<geometry_msgs::msg::Twist>();
msg->linear.x = std::numeric_limits<double>::quiet_NaN();
msg->linear.y = std::numeric_limits<double>::quiet_NaN();
msg->linear.z = std::numeric_limits<double>::quiet_NaN();
msg->angular.x = std::numeric_limits<double>::quiet_NaN();
msg->angular.y = std::numeric_limits<double>::quiet_NaN();
msg->angular.z = std::numeric_limits<double>::quiet_NaN();

return msg;
}

} // namespace
Expand Down Expand Up @@ -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<double>::quiet_NaN());
system_state_values_.assign(system_state_values_.size(), std::numeric_limits<double>::quiet_NaN());
Expand All @@ -244,7 +247,13 @@ 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 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<double> reference = {(*current_reference)->linear.x, (*current_reference)->linear.y,
(*current_reference)->linear.z, (*current_reference)->angular.x,
Expand All @@ -256,15 +265,21 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc
}
}

reset_twist_msg(*current_reference);
reference_.writeFromNonRT(empty_twist_msg());

return controller_interface::return_type::OK;
}

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 is incremented
auto current_system_state = system_state_.readFromRT();

// no command received yet
if (!current_system_state || !(*current_system_state)) {
return controller_interface::return_type::OK;
}

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,
Expand All @@ -276,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();
Expand Down