...

src/mtc/src/impl/mediator.cpp

@@ -128,6 +128,15 @@ void Mediator::mediate(){
     }
 
     calculate(ground_per_robot);
+
+    //swap
+    Abstract_robot* ar = robots_[1];
+    robots_[1] = robots_[0];
+    robots_[0] = ar;
+    
+    calculate(ground_per_robot);
+
+
 }
 
 void Mediator::calculate(std::vector<tf2::Transform>& ground_per_robot){

src/mtc/src/impl/moveit_mediator.cpp

@@ -74,7 +74,6 @@ void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
 
     for (std::size_t i = 0; i < result.size(); i++){
         if (result[i]){
-			ROS_INFO("%i", i);
             ceti->register_observers(wings_[robot][i]);
         }
     }
@@ -93,8 +92,6 @@ void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>,
 
 void Moveit_mediator::setup_task(){
 	ROS_INFO("=> write Task Constructor Objects");
-	ROS_INFO("=> ");
-
 	
 	bool coop = false;