...

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& 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& 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);
 	}