...

.catkin_tools/profiles/default/devel_collisions.txt

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

.catkin_tools/profiles/default/packages/mtc/package.xml

@@ -39,7 +39,6 @@
 
 
   <build_depend>pcl_ros</build_depend>
-  <build_depend>yaml_to_mtc</build_depend>
 
   
   <run_depend>filesystem</run_depend> 

src/mtc/CMakeLists.txt

@@ -37,7 +37,6 @@ find_package(catkin REQUIRED COMPONENTS
   tf_conversions
   trajectory_msgs
   pcl_ros
-  yaml_to_mtc
 )
 
 # Catkin

src/mtc/include/impl/moveit_mediator.h

@@ -72,7 +72,7 @@ class Moveit_mediator : public Abstract_mediator{
     void load_robot_description();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
     void parallel_exec(Abstract_robot& penis, moveit_msgs::CollisionObject& source, std::vector<tf2::Transform>& target);
-    void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
+    moveit::task_constructor::Task create_Task(Abstract_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/launch/cell_routine.launch

@@ -8,7 +8,7 @@
 
     <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/dummy2.yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
     
 
     <include file="$(find ceti_double)/launch/demo.launch"> 

src/mtc/package.xml

@@ -39,7 +39,6 @@
 
 
   <build_depend>pcl_ros</build_depend>
-  <build_depend>yaml_to_mtc</build_depend>
 
   
   <run_depend>filesystem</run_depend> 

src/mtc/src/impl/moveit_mediator.cpp

@@ -124,9 +124,16 @@ void Moveit_mediator::setup_task(){
 		
 		for( int i = 0; i < objects_.size();i++){
 			for(int j =0; j < objects_[i].size();j++){
-				create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]);
+				moveit::task_constructor::Task task = create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]);
+				if (task.plan(1)){
+					moveit_msgs::MoveItErrorCodes execute_result;
+					execute_result = task.execute(*task.solutions().front());
+					//task_.introspection().publishSolution(*task_.solutions().front());
 				}
+
 			}
+		}
+		
 	} else {
 		moveit_msgs::CollisionObject bottle1;
 		bottle1.id = "bottle1";
@@ -182,11 +189,11 @@ void Moveit_mediator::setup_task(){
 			create_Task(robots_[1], psi_->getObjects().at("bottle2"), objects_[1][j]);
 		}
 		*/
-		std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0]));
-		std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1]));
+		//std::thread one(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[0]), std::ref(psi_->getObjects().at("bottle1")), std::ref(objects_[0]));
+		//std::thread two(&Moveit_mediator::parallel_exec, this, std::ref(*robots_[1]), std::ref(psi_->getObjects().at("bottle2")), std::ref(objects_[1]));
 
-		one.join();
-		two.join();
+		//one.join();
+		//two.join();
 	}
 
 }
@@ -197,7 +204,7 @@ void Moveit_mediator::parallel_exec(Abstract_robot& ar, moveit_msgs::CollisionOb
 	}
 }
 
-void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){
+moveit::task_constructor::Task Moveit_mediator::create_Task(Abstract_robot* r, 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";
@@ -480,14 +487,7 @@ void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObjec
 		task_.add(std::move(stage));
 	}
 
-	//task_map_.at(mr->name()).push_back(std::move(task_));
-
-	if (task_.plan(1)){
-		moveit_msgs::MoveItErrorCodes execute_result;
-		execute_result = task_.execute(*task_.solutions().front());
-		//task_.introspection().publishSolution(*task_.solutions().front());
-	}
-
+	return task_;
 }