Handle asynchronous feedback

The grasp detection flag is an atomic bool. The scope of the
grasp candidate's mutex has been moved.

Author: bostoncleek

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<moveit_msgs::Grasp> grasp_candidates_;
+	std::mutex grasp_mutex_;  // Protects grasp candidates
+	std::atomic_bool found_candidates_;  // Flag indicates the discovery of grasps
+	std::vector<moveit_msgs::Grasp> grasp_candidates_;  // Grasp Candidates
 };
 }  // namespace stages
 }  // namespace task_constructor

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<std::mutex> lock(grasp_mutex_);
 	grasp_candidates_ = feedback->grasps;
-	found_candidates_ = true;
 }
 
 void GraspProvider::doneCallback(const actionlib::SimpleClientGoalState& state,
@@ -158,8 +160,9 @@ void GraspProvider::compute() {
 
 	// monitor feedback/results
 	// blocking function untill timeout reached or results received
-	const std::lock_guard<std::mutex> lock(grasp_mutex_);
 	if (monitorGoal()) {
+		// Protect grasp candidate incase feedback is being recieved asynchronously
+		const std::lock_guard<std::mutex> 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);