Fix Segfault in GripperActionController (#527)

* Use RT buffer for gripper action controller's goal handle

* fix format

---------

Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>
(cherry picked from commit 520036d)

gripper_controllers/include/gripper_controllers/gripper_action_controller.hpp

@@ -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>;
 
@@ -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_;

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -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());
   }
 }
 
@@ -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>
@@ -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();
@@ -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;
 }
@@ -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;
   }
@@ -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
   {
@@ -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());
     }
   }
 }