some changes for docker

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 25109
+/home/matteo/reachability/devel/./cmake.lock 25373
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79

src/mtc/include/impl/moveit_mediator.h

@@ -76,8 +76,8 @@ class Moveit_mediator : public Abstract_mediator{
     void publish_tables();
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
-    void parallel_exec(Abstract_robot& r, moveit::task_constructor::Task& task);
-    moveit::task_constructor::Task create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
+    void parallel_exec(Moveit_robot& r, moveit_msgs::CollisionObject source,  std::vector<tf2::Transform> target);
+    moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
     inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
 
 

src/mtc/src/impl/moveit_mediator.cpp

 #include "impl/moveit_mediator.h"
 #include <thread>
+#include <mutex> 
 
+thread_local moveit::task_constructor::Task task__;
+thread_local moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg_;
+
+std::mutex task_writing;
+std::mutex publish;
 
 
 void Moveit_mediator::publish_tables(){
@@ -124,7 +130,8 @@ void Moveit_mediator::setup_task(){
 		
 		for( int i = 0; i < objects_.size();i++){
 			for(int j =0; j < objects_[i].size();j++){
-				moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]);
+				Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
+				moveit::task_constructor::Task task = create_Task(mr, psi_->getObjects().at("bottle"), objects_[i][j]);
 				if (task.plan(1)){
 					moveit_msgs::MoveItErrorCodes execute_result;
 					execute_result = task.execute(*task.solutions().front());
@@ -180,35 +187,42 @@ void Moveit_mediator::setup_task(){
 		psi_->applyCollisionObject(bottle1);
 		psi_->applyCollisionObject(bottle2);
 
-
+		std::vector<std::thread> threads;
 
 		for (int i = 0 ; i < objects_.size(); i++){
 			std::stringstream ss;
 			ss << "bottle" << std::to_string(i);
-			for(int j =0; j < objects_[i].size();j++){
-				moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at(ss.str()), objects_[i][j]);
-				if (task.plan(1)){
-					parallel_exec(std::ref(*robots_[i]), std::ref(task));
-					//std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(task));
-					//one.join();
-				}
+			Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
+			threads.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
 		}
+
+		for (int i = 0; i < threads.size(); i++) threads[i].join();
 	}
 }
 	
-}
 
-void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit::task_constructor::Task& task){
-	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(&ar);
-	moveit_task_constructor_msgs::ExecuteTaskSolutionGoal etsg;
-	task.solutions().front()->fillMessage(etsg.solution, &task.introspection());
-	for(auto st : etsg.solution.sub_trajectory){
-		mr->mgi()->execute(st.trajectory);
-		planning_scene_diff_publisher_.publish(st.scene_diff);
+void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::CollisionObject source,  std::vector<tf2::Transform> target){
+	ros::Duration sleep(1.0);
+	for(int i = 0; i < target.size(); i++){
+		//task writing task
+		task_writing.lock();
+		task__ = create_Task(&mr, source, target[i]);
+		if (!task__.plan(1)) { continue; task_writing.unlock();}
+		task__.solutions().front()->fillMessage(etsg_.solution, &task__.introspection());
+		task_writing.unlock();
+
+		//Iterate throug trajetory
+		for(int j = 0; j <  etsg_.solution.sub_trajectory.size(); j++){
+			mr.mgi()->execute(etsg_.solution.sub_trajectory[j].trajectory);
+			sleep.sleep();
+			planning_scene_diff_publisher_.publish(etsg_.solution.sub_trajectory[j].scene_diff);
+			// secure publishing
+		}
 	}
 }
 
-moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
+
+moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, moveit_msgs::CollisionObject& source, tf2::Transform& target){
     tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z));
 	std::string support_surface1 = "nichts";
     std::string support_surface2 = "nichts";
@@ -227,8 +241,6 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, m
 	const std::string object = source.id;
     moveit::task_constructor::Task task_;
 
-	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
-
 	std::string name = "Pick&Place";
 	task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
 	task_.loadRobotModel();