From 9cd3fe069e6eb82e58a1a7c4813ce0ede06142a1 Mon Sep 17 00:00:00 2001 From: bostoncleek Date: Sun, 9 Jan 2022 14:59:45 -0500 Subject: [PATCH] Handle asynchronous feedback The grasp detection flag is an atomic bool. The scope of the grasp candidate's mutex has been moved. --- .../moveit/task_constructor/stages/grasp_provider.h | 6 +++--- core/src/stages/grasp_provider.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/grasp_provider.h b/core/include/moveit/task_constructor/stages/grasp_provider.h index 3af040b72..9d1d94103 100644 --- a/core/include/moveit/task_constructor/stages/grasp_provider.h +++ b/core/include/moveit/task_constructor/stages/grasp_provider.h @@ -98,9 +98,9 @@ class GraspProvider : public GeneratePose, ActionBase void onNewSolution(const SolutionBase& s) override; private: - std::mutex grasp_mutex_; - bool found_candidates_; - std::vector grasp_candidates_; + std::mutex grasp_mutex_; // Protects grasp candidates + std::atomic_bool found_candidates_; // Flag indicates the discovery of grasps + std::vector grasp_candidates_; // Grasp Candidates }; } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/grasp_provider.cpp b/core/src/stages/grasp_provider.cpp index 95767bfe3..25ae270a4 100644 --- a/core/src/stages/grasp_provider.cpp +++ b/core/src/stages/grasp_provider.cpp @@ -94,9 +94,11 @@ void GraspProvider::activeCallback() { } void GraspProvider::feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) { + found_candidates_ = true; + + // Protect grasp candidate incase feedback is sent asynchronously const std::lock_guard lock(grasp_mutex_); grasp_candidates_ = feedback->grasps; - found_candidates_ = true; } void GraspProvider::doneCallback(const actionlib::SimpleClientGoalState& state, @@ -158,8 +160,10 @@ void GraspProvider::compute() { // monitor feedback/results // blocking function untill timeout reached or results received - const std::lock_guard lock(grasp_mutex_); if (monitorGoal()) { + + // Protect grasp candidate incase feedback is being recieved asynchronously + const std::lock_guard lock(grasp_mutex_); for (unsigned int i = 0; i < grasp_candidates_.size(); i++) { InterfaceState state(scene); state.properties().set("target_pose", grasp_candidates_.at(i).grasp_pose);