From b326d22f222e4403e7fbb35fc77488a415a5b617 Mon Sep 17 00:00:00 2001 From: Prathmesh Atkale Date: Mon, 24 Nov 2025 16:22:36 +0530 Subject: [PATCH] move_group_interface: make async callbacks safe and track active goal handles Replace stack-captured state in plan(), move(), and execute() with heap-shared state captured by value in callbacks to avoid use-after-free when async methods return. Add mutex-protected `active_move_goal_handle_` and `active_execute_goal_handle_` members and cancel active goals in the destructor. Include and . --- .../src/move_group_interface.cpp | 120 +++++++++++++----- 1 file changed, 90 insertions(+), 30 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 40e3fbdeed..5d85a0426a 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include #include @@ -213,6 +215,23 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (constraints_init_thread_) constraints_init_thread_->join(); + // Attempt to cancel any active goals to avoid callbacks accessing destroyed state + try + { + std::lock_guard sl(active_goal_mutex_); + if (move_action_client_ && active_move_goal_handle_) + { + move_action_client_->async_cancel_goal(active_move_goal_handle_); + } + if (execute_action_client_ && active_execute_goal_handle_) + { + execute_action_client_->async_cancel_goal(active_execute_goal_handle_); + } + } + catch (...) + { + } + callback_executor_.cancel(); if (callback_thread_.joinable()) @@ -671,26 +690,38 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal.planning_options.planning_scene_diff.is_diff = true; goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - bool done = false; - rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; - std::shared_ptr res; + struct PlanState + { + std::atomic done{false}; + rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; + std::shared_ptr res; + }; + auto state = std::make_shared(); auto send_goal_opts = rclcpp_action::Client::SendGoalOptions(); send_goal_opts.goal_response_callback = - [&](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, state](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (!goal_handle) { - done = true; + state->done = true; RCLCPP_INFO(logger_, "Planning request rejected"); } else + { RCLCPP_INFO(logger_, "Planning request accepted"); + std::lock_guard sl(active_goal_mutex_); + active_move_goal_handle_ = goal_handle; + } }; send_goal_opts.result_callback = - [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - res = result.result; - code = result.code; - done = true; + [this, state](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + state->res = result.result; + state->code = result.code; + { + std::lock_guard sl(active_goal_mutex_); + active_move_goal_handle_.reset(); + } + state->done = true; switch (result.code) { @@ -712,7 +743,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts); // wait until send_goal_opts.result_callback is called - while (!done) + while (!state->done) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } @@ -748,26 +779,38 @@ class MoveGroupInterface::MoveGroupInterfaceImpl goal.planning_options.planning_scene_diff.is_diff = true; goal.planning_options.planning_scene_diff.robot_state.is_diff = true; - bool done = false; - rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; - std::shared_ptr res; + struct MoveState + { + std::atomic done{false}; + rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; + std::shared_ptr res; + }; + auto state = std::make_shared(); auto send_goal_opts = rclcpp_action::Client::SendGoalOptions(); send_goal_opts.goal_response_callback = - [&](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, state](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (!goal_handle) { - done = true; + state->done = true; RCLCPP_INFO(logger_, "Plan and Execute request rejected"); } else + { RCLCPP_INFO(logger_, "Plan and Execute request accepted"); + std::lock_guard sl(active_goal_mutex_); + active_move_goal_handle_ = goal_handle; + } }; send_goal_opts.result_callback = - [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - res = result.result; - code = result.code; - done = true; + [this, state](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + state->res = result.result; + state->code = result.code; + { + std::lock_guard sl(active_goal_mutex_); + active_move_goal_handle_.reset(); + } + state->done = true; switch (result.code) { @@ -790,7 +833,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return moveit::core::MoveItErrorCode::SUCCESS; // wait until send_goal_opts.result_callback is called - while (!done) + while (!state->done) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } @@ -811,26 +854,38 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return moveit::core::MoveItErrorCode::FAILURE; } - bool done = false; - rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; - std::shared_ptr res; + struct ExecuteState + { + std::atomic done{false}; + rclcpp_action::ResultCode code = rclcpp_action::ResultCode::UNKNOWN; + std::shared_ptr res; + }; + auto state = std::make_shared(); auto send_goal_opts = rclcpp_action::Client::SendGoalOptions(); send_goal_opts.goal_response_callback = - [&](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [this, state](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { if (!goal_handle) { - done = true; + state->done = true; RCLCPP_INFO(logger_, "Execute request rejected"); } else + { RCLCPP_INFO(logger_, "Execute request accepted"); + std::lock_guard sl(active_goal_mutex_); + active_execute_goal_handle_ = goal_handle; + } }; send_goal_opts.result_callback = - [&](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - res = result.result; - code = result.code; - done = true; + [this, state](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + state->res = result.result; + state->code = result.code; + { + std::lock_guard sl(active_goal_mutex_); + active_execute_goal_handle_.reset(); + } + state->done = true; switch (result.code) { @@ -858,7 +913,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return moveit::core::MoveItErrorCode::SUCCESS; // wait until send_goal_opts.result_callback is called - while (!done) + while (!state->done) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } @@ -1223,6 +1278,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl // std::shared_ptr> place_action_client_; std::shared_ptr> execute_action_client_; + // Active goal handles (protected by mutex). Stored so they can be cancelled on destruction + std::mutex active_goal_mutex_; + rclcpp_action::ClientGoalHandle::SharedPtr active_move_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr active_execute_goal_handle_; + // general planning params moveit_msgs::msg::RobotState considered_start_state_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_;