From 7604cd5c9a21adea9f18ef7b434f0da710f8a331 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Tue, 21 Feb 2023 17:00:04 +0100 Subject: [PATCH] Fix Segfault in GripperActionController (#527) (#530) * Use RT buffer for gripper action controller's goal handle * fix format --------- Co-authored-by: Erik Holum --- .../gripper_action_controller.hpp | 4 +- .../gripper_action_controller_impl.hpp | 57 ++++++++++--------- 2 files changed, 32 insertions(+), 29 deletions(-) diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp index 830de7b514..89bf232fad 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp @@ -115,6 +115,7 @@ class GripperActionController : public controller_interface::ControllerInterface using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; using HwIfaceAdapter = HardwareInterfaceAdapter; @@ -134,7 +135,8 @@ class GripperActionController : public controller_interface::ControllerInterface HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface. - RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any. + RealtimeGoalHandleBuffer + rt_active_goal_; ///< Container for the currently active action goal, if any. control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_; rclcpp::Duration action_monitor_period_; diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp index 629f749050..666fced1c0 100644 --- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp +++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp @@ -28,11 +28,12 @@ template void GripperActionController::preempt_active_goal() { // Cancels the currently active goal - if (rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal) { // Marks the current goal as canceled - rt_active_goal_->setCanceled(std::make_shared()); - rt_active_goal_.reset(); + active_goal->setCanceled(std::make_shared()); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } @@ -86,25 +87,23 @@ template void GripperActionController::accepted_callback( std::shared_ptr goal_handle) // Try to update goal { - { - auto rt_goal = std::make_shared(goal_handle); + auto rt_goal = std::make_shared(goal_handle); - // Accept new goal - preempt_active_goal(); + // Accept new goal + preempt_active_goal(); - // This is the non-realtime command_struct - // We use command_ for sharing - command_struct_.position_ = goal_handle->get_goal()->command.position; - command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; - command_.writeFromNonRT(command_struct_); + // This is the non-realtime command_struct + // We use command_ for sharing + command_struct_.position_ = goal_handle->get_goal()->command.position; + command_struct_.max_effort_ = goal_handle->get_goal()->command.max_effort; + command_.writeFromNonRT(command_struct_); - pre_alloc_result_->reached_goal = false; - pre_alloc_result_->stalled = false; + pre_alloc_result_->reached_goal = false; + pre_alloc_result_->stalled = false; - last_movement_time_ = get_node()->now(); - rt_active_goal_ = rt_goal; - rt_active_goal_->execute(); - } + last_movement_time_ = get_node()->now(); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); @@ -112,7 +111,7 @@ void GripperActionController::accepted_callback( // Setup goal status checking timer goal_handle_timer_ = get_node()->create_wall_timer( action_monitor_period_.to_chrono(), - std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_)); + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); } template @@ -122,7 +121,8 @@ rclcpp_action::CancelResponse GripperActionController::cancel RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); // Check that cancel request refers to currently active goal (if any) - if (rt_active_goal_ && rt_active_goal_->gh_ == goal_handle) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { // Enter hold current position mode set_hold_position(); @@ -132,9 +132,9 @@ rclcpp_action::CancelResponse GripperActionController::cancel // Mark the current goal as canceled auto action_res = std::make_shared(); - rt_active_goal_->setCanceled(action_res); + active_goal->setCanceled(action_res); // Reset current goal - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } return rclcpp_action::CancelResponse::ACCEPT; } @@ -152,7 +152,8 @@ void GripperActionController::check_for_success( const rclcpp::Time & time, double error_position, double current_position, double current_velocity) { - if (!rt_active_goal_) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (!active_goal) { return; } @@ -164,8 +165,8 @@ void GripperActionController::check_for_success( pre_alloc_result_->reached_goal = true; pre_alloc_result_->stalled = false; RCLCPP_DEBUG(get_node()->get_logger(), "Successfully moved to goal."); - rt_active_goal_->setSucceeded(pre_alloc_result_); - rt_active_goal_.reset(); + active_goal->setSucceeded(pre_alloc_result_); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } else { @@ -183,14 +184,14 @@ void GripperActionController::check_for_success( if (params_.allow_stalling) { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Returning success."); - rt_active_goal_->setSucceeded(pre_alloc_result_); + active_goal->setSucceeded(pre_alloc_result_); } else { RCLCPP_DEBUG(get_node()->get_logger(), "Stall detected moving to goal. Aborting action!"); - rt_active_goal_->setAborted(pre_alloc_result_); + active_goal->setAborted(pre_alloc_result_); } - rt_active_goal_.reset(); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } } }