Skip to content

Commit

Permalink
Fix Segfault in GripperActionController (#527) (#530)
Browse files Browse the repository at this point in the history
* Use RT buffer for gripper action controller's goal handle

* fix format

---------

Co-authored-by: Erik Holum <erik.holum@picknik.ai>
  • Loading branch information
bmagyar and Erik Holum authored Feb 21, 2023
1 parent 5713a41 commit 7604cd5
Show file tree
Hide file tree
Showing 2 changed files with 32 additions and 29 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ class GripperActionController : public controller_interface::ControllerInterface
using RealtimeGoalHandle =
realtime_tools::RealtimeServerGoalHandle<control_msgs::action::GripperCommand>;
using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;

using HwIfaceAdapter = HardwareInterfaceAdapter<HardwareInterface>;

Expand All @@ -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_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,12 @@ template <const char * HardwareInterface>
void GripperActionController<HardwareInterface>::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<GripperCommandAction::Result>());
rt_active_goal_.reset();
active_goal->setCanceled(std::make_shared<GripperCommandAction::Result>());
rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
}
}

Expand Down Expand Up @@ -86,33 +87,31 @@ template <const char * HardwareInterface>
void GripperActionController<HardwareInterface>::accepted_callback(
std::shared_ptr<GoalHandle> goal_handle) // Try to update goal
{
{
auto rt_goal = std::make_shared<RealtimeGoalHandle>(goal_handle);
auto rt_goal = std::make_shared<RealtimeGoalHandle>(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();

// Setup goal status checking timer
goal_handle_timer_ = get_node()->create_wall_timer(
action_monitor_period_.to_chrono<std::chrono::nanoseconds>(),
std::bind(&RealtimeGoalHandle::runNonRealtime, rt_active_goal_));
std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal));
}

template <const char * HardwareInterface>
Expand All @@ -122,7 +121,8 @@ rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::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();
Expand All @@ -132,9 +132,9 @@ rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::cancel

// Mark the current goal as canceled
auto action_res = std::make_shared<GripperCommandAction::Result>();
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;
}
Expand All @@ -152,7 +152,8 @@ void GripperActionController<HardwareInterface>::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;
}
Expand All @@ -164,8 +165,8 @@ void GripperActionController<HardwareInterface>::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
{
Expand All @@ -183,14 +184,14 @@ void GripperActionController<HardwareInterface>::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());
}
}
}
Expand Down

0 comments on commit 7604cd5

Please sign in to comment.