Use CallbackGroup for MoveGroupSequenceAction

moveit · Dec 3, 2021 · ebf83c3 · ebf83c3
1 parent ad489f6
commit ebf83c3
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 14 deletions.
diff --git a/...strial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h b/...strial_motion_planner/include/pilz_industrial_motion_planner/move_group_sequence_action.h
@@ -44,6 +44,7 @@
 namespace pilz_industrial_motion_planner
 {
 class CommandListManager;
+using MoveGroupSequenceGoalHandle = rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>;
 
 /**
  * @brief Provide action to handle multiple trajectories and execute the result
@@ -64,8 +65,7 @@ class MoveGroupSequenceAction : public move_group::MoveGroupCapability
   using PlannedTrajMsgs = moveit_msgs::msg::MotionSequenceResponse::_planned_trajectories_type;
 
 private:
-  void executeSequenceCallback(
-      const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>> goal_handle);
+  void executeSequenceCallback(const std::shared_ptr<MoveGroupSequenceGoalHandle> goal_handle);
   void
   executeSequenceCallbackPlanAndExecute(const moveit_msgs::action::MoveGroupSequence::Goal::ConstSharedPtr& goal,
                                         const moveit_msgs::action::MoveGroupSequence::Result::SharedPtr& action_res);
@@ -83,8 +83,9 @@ class MoveGroupSequenceAction : public move_group::MoveGroupCapability
                            PlannedTrajMsgs& plannedTrajsMsgs);
 
 private:
+  rclcpp::CallbackGroup::SharedPtr action_callback_group_;
   std::shared_ptr<rclcpp_action::Server<moveit_msgs::action::MoveGroupSequence>> move_action_server_;
-  std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>> goal_handle_;
+  std::shared_ptr<MoveGroupSequenceGoalHandle> goal_handle_;
   moveit_msgs::action::MoveGroupSequence::Feedback::SharedPtr move_feedback_;
 
   move_group::MoveGroupState move_state_{ move_group::IDLE };

diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp
@@ -56,6 +56,7 @@ namespace pilz_industrial_motion_planner
 {
 static const rclcpp::Logger LOGGER =
     rclcpp::get_logger("moveit.pilz_industrial_motion_planner.move_group_sequence_action");
+
 MoveGroupSequenceAction::MoveGroupSequenceAction() : MoveGroupCapability("SequenceAction")
 {
 }
@@ -64,33 +65,31 @@ void MoveGroupSequenceAction::initialize()
 {
   // start the move action server
   RCLCPP_INFO_STREAM(LOGGER, "initialize move group sequence action");
+  action_callback_group_ =
+      context_->moveit_cpp_->getNode()->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
   move_action_server_ = rclcpp_action::create_server<moveit_msgs::action::MoveGroupSequence>(
       context_->moveit_cpp_->getNode(), "sequence_move_group",
       [](const rclcpp_action::GoalUUID& /* unused */,
          std::shared_ptr<const moveit_msgs::action::MoveGroupSequence::Goal> /* unused */) {
         RCLCPP_DEBUG(LOGGER, "Received action goal");
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [this](const std::shared_ptr<
-             rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>> /* unused goal_handle */) {
+      [this](const std::shared_ptr<MoveGroupSequenceGoalHandle> /* unused goal_handle */) {
         RCLCPP_DEBUG(LOGGER, "Canceling action goal");
         preemptMoveCallback();
         return rclcpp_action::CancelResponse::ACCEPT;
       },
-      [this](
-          const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>> goal_handle) {
-        // Return quickly to avoid blocking executor
-        std::thread(std::bind(&MoveGroupSequenceAction::executeSequenceCallback, this, std::placeholders::_1),
-                    goal_handle)
-            .detach();
-      });
+      [this](const std::shared_ptr<MoveGroupSequenceGoalHandle> goal_handle) {
+        RCLCPP_DEBUG(LOGGER, "Accepting new action goal");
+        executeSequenceCallback(goal_handle);
+      },
+      rcl_action_server_get_default_options(), action_callback_group_);
 
   command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(
       context_->moveit_cpp_->getNode(), context_->planning_scene_monitor_->getRobotModel());
 }
 
-void MoveGroupSequenceAction::executeSequenceCallback(
-    const std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_msgs::action::MoveGroupSequence>> goal_handle)
+void MoveGroupSequenceAction::executeSequenceCallback(const std::shared_ptr<MoveGroupSequenceGoalHandle> goal_handle)
 {
   // Notify that goal is being executed
   goal_handle_ = goal_handle;