From b1f20d50b3da24c7b53a80946c84815f1a50f526 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Fri, 10 Feb 2023 00:19:15 +0100
Subject: [PATCH] ...

---
 src/impl/moveit_mediator.cpp | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index da91c63..b474936 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -903,30 +903,30 @@ void Moveit_mediator::task_planner(){
 			for (int i = 0; i < robots_.size(); i++){
 				if (exec.first == robots_[i]->name()){
 					auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
-					//th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first));
-					mr->mgi()->execute(exec.second.first);
+					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), exec.second.first));
+					//mr->mgi()->execute(exec.second.first);
 					//manipulate_acm(mr, exec.second.second);
 					//merge_ps(ps_m, exec.second.second, mr);
 				}
 			}
 		}
 		
-		// for(auto& t : th){
-		// 	t.join();
-		// }
-
-		// for(auto& exec : executions_){
-		//  	for (int i = 0; i < robots_.size(); i++){
-		//  		if (exec.first == robots_[i]->name()){
-		//  			auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
-	 	// 			manipulate_acm(mr, exec.second.second);
-		//  			merge_ps(ps_m, exec.second.second, mr);
-		//  		}
-		// 	}
-		// }
+		for(auto& t : th){
+			t.join();
+		}
+
+		for(auto& exec : executions_){
+		 	for (int i = 0; i < robots_.size(); i++){
+		 		if (exec.first == robots_[i]->name()){
+		 			auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
+	 				manipulate_acm(mr, exec.second.second);
+		 			merge_ps(ps_m, exec.second.second, mr);
+		 		}
+			}
+		}
 
 		// merge_acm(ps_m);
-		// planning_scene_diff_publisher_->publish(ps_m);
+		planning_scene_diff_publisher_->publish(ps_m);
 	}
 
 
-- 
GitLab