Skip to content
Snippets Groups Projects
Select Git revision
  • 5a84c46c14c1a7d9ed21f89e53376d731f7d838b
  • master default protected
2 results

MqttRosConnectionTestNode.cpp

Blame
  • moveit_mediator.cpp 59.99 KiB
    #include "impl/moveit_mediator.h"
    #include <thread>
    #include <mutex> 
    #include <algorithm>
    #include <gb_grasp/MapGenerator.h>
    #include <regex>
    
    #include "behaviortree_cpp_v3/behavior_tree.h"
    #include "behaviortree_cpp_v3/tree_node.h"
    #include "behaviortree_cpp_v3/bt_factory.h"
    #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
    #include "bt/planning_scene.h"
    #include "bt/trajetory.h"
    #include "bt/execution.h"
    #include "bt/position_condition.h"
    #include "bt/parallel_robot.h"
    #include <actionlib_msgs/GoalStatusArray.h>
    
    
    
    
    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::connect_robots(Abstract_robot* robot) {
    	robots_.push_back(robot);
    	ROS_INFO("%s connected...", robot->name().c_str());
    
    	Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robot);
    	acm_.insert(std::pair<std::string, std::vector<uint8_t>>(mr->map().at("base").c_str(), std::vector<uint8_t>()));
    	for(auto name: mr->mgi()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
    	for(auto name: mr->mgi_hand()->getLinkNames()) acm_.insert(std::pair<std::string, std::vector<uint8_t>>(name.c_str(), std::vector<uint8_t>()));
    }
    
    void Moveit_mediator::publish_tables(){
        ros::WallDuration sleep_time(1.0);
        for(long unsigned int i = 0; i < robots_.size();i++){
            Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
    
    		/*
    		moveit_msgs::CollisionObject table;
    		table.id = ceti->map().at("base").c_str();
    		table.header.frame_id = "world";
    		table.header.stamp = ros::Time::now();
    		table.primitives.resize(1);
    		table.primitives[0].type = table.primitives[0].BOX;
    		table.primitives[0].dimensions.resize(3);
    		table.primitives[0].dimensions[0] = ceti->size().getX();
    		table.primitives[0].dimensions[1] = ceti->size().getY();
    		table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2;
    		table.primitive_poses.resize(1);
    		table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX();
    		table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY();
    		table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ();
    		table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX();
    		table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY();
    		table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ();
    		table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW();
    		table.operation = table.ADD;
    		*/
    		//psi_->applyCollisionObject(table);
    
            for (Abstract_robot_element* are : ceti->observers()) {
                Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are);
                psi_->applyCollisionObject(*wmd->markers());
    			acm_.insert(std::pair<std::string, std::vector<uint8_t>>(wmd->markers()->id.c_str(), std::vector<uint8_t>()));
                sleep_time.sleep();
            }
        }
    }
    
    bool Moveit_mediator::check_collision(const int& robot){
        auto env = ps_->getCollisionEnvNonConst();
        Collision_helper* ch = static_cast<Collision_helper*>(env.get());
        return ch->collision_detector("object_A", "object_B"); //
    }
    
    void Moveit_mediator::mediate(){
        publish_tables();
    	task_planner();
    	setup_task();	
    	ros::waitForShutdown();
    
    
    };
    
    void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
        std::bitset<3> result = robots_[robot]->observer_mask() & wing;
        Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
    
        for (std::size_t i = 0; i < result.size(); i++){
            if (result[i]){
                ceti->register_observers(wings_[robot][i]);
            }
        }
    }
    
    void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
        for (int i =0; i < wbp.size(); i++){
            std::vector<Abstract_robot_element*> v;
            for (int j =0; j < wbp[i].first.size(); j++){
                Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
                v.push_back(are);
            }
            wings_.push_back(v);
        }
    };
    
    void Moveit_mediator::setup_task(){
    	ROS_INFO("=> write Task Constructor Objects");
    	for(auto& a: acm_) a.second.resize(acm_.size(), 0);
    	/*
    	
    	bool coop = false;
    
    	for(int i = 0; i < objects_.size(); i++){
            if (i+1 < objects_.size()){
                for (long unsigned int j = objects_[i].size()-1; j > 0; j--){
                    if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) {
    					objects_[i+1].pop_back(); 
    					coop= true;
    				}
                }
            }
        }
    
    	if(coop){
    		moveit_msgs::CollisionObject bottle;
    		bottle.id = "bottle";
    		bottle.header.frame_id = "world";
    		bottle.header.stamp = ros::Time::now();
    		bottle.primitives.resize(1);
    		bottle.primitives[0].type = bottle.primitives[0].BOX;
    		bottle.primitives[0].dimensions.resize(3);
    		bottle.primitives[0].dimensions[0] = box_size.getX();
    		bottle.primitives[0].dimensions[1] = box_size.getY();
    		bottle.primitives[0].dimensions[2] = box_size.getZ();
    
    		bottle.primitive_poses.resize(1);
    		bottle.primitive_poses[0].position.x = -0.3f;
    		bottle.primitive_poses[0].position.y = -0.6f;
    		bottle.primitive_poses[0].position.z = 0.9355f;
    		bottle.primitive_poses[0].orientation.x = 0;
    		bottle.primitive_poses[0].orientation.y = 0;
    		bottle.primitive_poses[0].orientation.z = 0;
    		bottle.primitive_poses[0].orientation.w = 1;
    		bottle.operation = bottle.ADD;
    
    		psi_->applyCollisionObject(bottle);
    
    		
    		for( int i = 0; i < objects_.size();i++){
    			for(int j =0; j < objects_[i].size();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)){
    					task.execute(*task.solutions().front());
    				}
    				
    			}
    		}
    	} else {
    		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle0", std::vector<uint8_t>()));
    		acm_.insert(std::pair<std::string, std::vector<uint8_t>>("bottle1", std::vector<uint8_t>()));
    
    		moveit_msgs::CollisionObject bottle1;
    		bottle1.id = "bottle0";
    		bottle1.header.frame_id = "world";
    		bottle1.header.stamp = ros::Time::now();
    		bottle1.primitives.resize(1);
    		bottle1.primitives[0].type = bottle1.primitives[0].BOX;
    		bottle1.primitives[0].dimensions.resize(3);
    		bottle1.primitives[0].dimensions[0] = box_size.getX();
    		bottle1.primitives[0].dimensions[1] = box_size.getY();
    		bottle1.primitives[0].dimensions[2] = box_size.getZ();
    
    		bottle1.primitive_poses.resize(1);
    		bottle1.primitive_poses[0].position.x = objects_[0][0].getOrigin().getX();
    		bottle1.primitive_poses[0].position.y = objects_[0][0].getOrigin().getY();
    		bottle1.primitive_poses[0].position.z = 0.9355f;
    		bottle1.primitive_poses[0].orientation.x = 0;
    		bottle1.primitive_poses[0].orientation.y = 0;
    		bottle1.primitive_poses[0].orientation.z = 0;
    		bottle1.primitive_poses[0].orientation.w = 1;
    		bottle1.operation = bottle1.ADD;
    
    		moveit_msgs::CollisionObject bottle2;
    		bottle2.id = "bottle1";
    		bottle2.header.frame_id = "world";
    		bottle2.header.stamp = ros::Time::now();
    		bottle2.primitives.resize(1);
    		bottle2.primitives[0].type = bottle2.primitives[0].BOX;
    		bottle2.primitives[0].dimensions.resize(3);
    		bottle2.primitives[0].dimensions[0] = box_size.getX();
    		bottle2.primitives[0].dimensions[1] = box_size.getY();
    		bottle2.primitives[0].dimensions[2] = box_size.getZ();
    
    		bottle2.primitive_poses.resize(1);
    		bottle2.primitive_poses[0].position.x = objects_[1][0].getOrigin().getX(); 
    		bottle2.primitive_poses[0].position.y = objects_[1][0].getOrigin().getY();
    		bottle2.primitive_poses[0].position.z = 0.9355f;
    		bottle2.primitive_poses[0].orientation.x = 0;
    		bottle2.primitive_poses[0].orientation.y = 0;
    		bottle2.primitive_poses[0].orientation.z = 0;
    		bottle2.primitive_poses[0].orientation.w = 1;
    		bottle2.operation = bottle2.ADD;
    
    		psi_->applyCollisionObject(bottle1);
    		psi_->applyCollisionObject(bottle2);
    
    		std::vector<std::thread> ths;
    		std::vector<std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> tasks;
    
    		for (int i = 0 ; i < objects_.size(); i++){
    			std::stringstream ss;
    			ss << "bottle" << std::to_string(i);
    			Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[i]);
    			std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
    			for (int j = 1; j < objects_[i].size(); j++){
    				moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(ss.str()), objects_[i][j]);
    				if(mgt.plan(1)) {
    					moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
    					mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
    					tasks_per_robot.push(e);
    
    					moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
    					temp.operation = temp.MOVE;
    					temp.pose.position.x = objects_[i][j].getOrigin().getX();
    					temp.pose.position.y = objects_[i][j].getOrigin().getY();
    					psi_->applyCollisionObject(temp);
    				}		
    			}
    
    			moveit_msgs::CollisionObject temp = psi_->getObjects().at(ss.str());
    			temp.operation = temp.MOVE;
    			temp.pose.position.x = objects_[i][0].getOrigin().getX();
    			temp.pose.position.y = objects_[i][0].getOrigin().getY();
    			psi_->applyCollisionObject(temp);
    			tasks.push_back(tasks_per_robot);
    			//ths.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), psi_->getObjects().at(ss.str()), objects_[i]));
    		}
    
    		for(auto& a: acm_) a.second.resize(acm_.size(), 0);
    		
    		while (!tasks[0].empty() && !tasks[1].empty()){
    			for (int i =0; i < std::max(tasks[0].front().solution.sub_trajectory.size(), tasks[1].front().solution.sub_trajectory.size()); i++){
    				moveit_msgs::RobotTrajectory* t1 = nullptr;
    				moveit_msgs::RobotTrajectory* t2 = nullptr;
    				moveit_msgs::PlanningScene* ps1 = nullptr;
    				moveit_msgs::PlanningScene* ps2 = nullptr;
    				moveit_msgs::PlanningScene ps_m;
    				std::vector<moveit_msgs::PlanningScene::_allowed_collision_matrix_type> acm_m;
    
    				//for(auto entry: acm_) ROS_INFO("%s", entry.first.c_str());
    
    				if (i < tasks[0].front().solution.sub_trajectory.size()){
    					t1 = &tasks[0].front().solution.sub_trajectory[i].trajectory;
    					ps1 = &tasks[0].front().solution.sub_trajectory[i].scene_diff;
    				} 
    
    				if (i < tasks[1].front().solution.sub_trajectory.size()){
    					t2 = &tasks[1].front().solution.sub_trajectory[i].trajectory;
    					ps2 = &tasks[1].front().solution.sub_trajectory[i].scene_diff;
    				}
    
    				std::vector<std::thread> th;
    
    				if (t1){
    					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]);
    					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t1, *ps1));
    
    					// First find ID from panda to start with
    					std::regex rx_panda("panda_arm([0-9]+)");
        				std::smatch match;
            			std::regex_match(mr->name(), match, rx_panda);
    
    					// build panda link regex
    					std::stringstream ss;
    					ss << "panda_" << match[1] << "_.*";
    					std::regex rx_panda_links(ss.str());
    					std::regex rx_box("bottle.*");
    					std::regex rx_table("table.*");
    
    					// Iterate task collsion matrix
    					for(int j = 0; j < ps1->allowed_collision_matrix.entry_names.size(); j++ ){
    						if( mr->map().at("base") == ps1->allowed_collision_matrix.entry_names[j]){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
    							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values								
    								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
    							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values							
    								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_box)){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j].c_str(), j);
    							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values							
    								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps1->allowed_collision_matrix.entry_names[j], match, rx_table)){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps1->allowed_collision_matrix.entry_names[j], j);
    							for(int k = 0; k < ps1->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values									
    								int distance = std::distance(acm_.begin(),acm_.find(ps1->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps1->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps1->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps1->allowed_collision_matrix.entry_names[k])[distance] = ps1->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    					}
    					merge_ps(ps_m, ps1, mr);
    				}
    
    				if (t2){
    					Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[1]);
    					th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *t2, *ps2));		
    
    					std::regex rx_panda("panda_arm([0-9]+)");
        				std::smatch match;
            			std::regex_match(mr->name(), match, rx_panda);
    
    					// build panda link regex
    					std::stringstream ss;
    					ss << "panda_" << match[1] << "_.*";
    					std::regex rx_panda_links(ss.str());
    					std::regex rx_box("bottle.*");
    					std::regex rx_table("table.*");
    
    					// Iterate task collsion matrix
    					for(int j = 0; j < ps2->allowed_collision_matrix.entry_names.size(); j++ ){
    						if( mr->map().at("base") == ps2->allowed_collision_matrix.entry_names[j]){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
    							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values						
    								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
    							In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
    							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								For that specific entry, loop over values								
    								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_box)){
    							In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j].c_str(), j);
    							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								For that specific entry, loop over values		
    								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    
    						if(std::regex_match(ps2->allowed_collision_matrix.entry_names[j], match, rx_table)){
    							 In case an entry matches an robot spezific link
    							ROS_INFO("entry matches link %s at index %i", ps2->allowed_collision_matrix.entry_names[j], j);
    							for(int k = 0; k < ps2->allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    								 For that specific entry, loop over values							
    								int distance = std::distance(acm_.begin(),acm_.find(ps2->allowed_collision_matrix.entry_names[k]));								
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    									ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_box)){
    									ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}				
    
    								if (std::regex_match(ps2->allowed_collision_matrix.entry_names[k], match, rx_table)){
    									ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}	
    
    								if (mr->map().at("base")== ps2->allowed_collision_matrix.entry_names[k]){
    									ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    									acm_.at(ps2->allowed_collision_matrix.entry_names[k])[distance] = ps2->allowed_collision_matrix.entry_values[j].enabled[k];
    								}
    							}
    						}
    					}
    					merge_ps(ps_m, ps2, mr);
    				}
    
    
    				for(int i = 0; i < th.size(); i++){
    					th[i].join();
    				}
    				merge_acm(ps_m);
    				planning_scene_diff_publisher_->publish(ps_m);
    
    			}
    			tasks[0].pop();
    			tasks[1].pop();
    		}
    	}
    	*/
    
    }
    
    void Moveit_mediator::manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps){
    	// First find ID from panda to start with
    	std::regex rx_panda("panda_arm([0-9]+)");
    	std::smatch match;
    	std::regex_match(mr->name(), match, rx_panda);
    
    	// build panda link regex
    	std::stringstream ss;
    	ss << "panda_" << match[1] << "_.*";
    	std::regex rx_panda_links(ss.str());
    	std::regex rx_box("box.*");
    	std::regex rx_table("table.*");
    
    	// Iterate task collsion matrix
    	for(int j = 0; j < ps.allowed_collision_matrix.entry_names.size(); j++ ){
    		if( mr->map().at("base") == ps.allowed_collision_matrix.entry_names[j]){
    			//In case an entry matches an robot spezific link
    			ROS_INFO("entry matches link %s at index %i", mr->map().at("base").c_str(), j);
    
    			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    				// For that specific entry, loop over values								
    				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
    					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}				
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
    					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}	
    
    				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
    					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    			}
    		}
    
    		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_panda_links)){
    			//In case an entry matches an robot spezific link
    			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
    			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    					//For that specific entry, loop over values							
    				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
    					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}				
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
    					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}	
    
    				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
    					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    			}
    		}
    
    		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_box)){
    				//In case an entry matches an robot spezific link
    			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j].c_str(), j);
    			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    				//For that specific entry, loop over values							
    				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
    					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}				
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
    					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}	
    
    				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
    					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    			}
    		}
    
    		if(std::regex_match(ps.allowed_collision_matrix.entry_names[j], match, rx_table)){
    			//In case an entry matches an robot spezific link
    			ROS_INFO("entry matches link %s at index %i", ps.allowed_collision_matrix.entry_names[j], j);
    			for(int k = 0; k < ps.allowed_collision_matrix.entry_values[j].enabled.size(); k++){
    				//For that specific entry, loop over values									
    				int distance = std::distance(acm_.begin(),acm_.find(ps.allowed_collision_matrix.entry_names[k]));								
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_panda_links)){
    					ROS_INFO("Entry in row corresponds to link_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_box)){
    					ROS_INFO("Entry in row corresponds to box_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}				
    
    				if (std::regex_match(ps.allowed_collision_matrix.entry_names[k], match, rx_table)){
    					ROS_INFO("Entry in row corresponds to table_pattern %s", match[0].str().c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}	
    
    				if (mr->map().at("base")== ps.allowed_collision_matrix.entry_names[k]){
    					ROS_INFO("Entry in row corresponds to base %s", mr->map().at("base").c_str());
    					acm_.at(ps.allowed_collision_matrix.entry_names[k])[distance] = ps.allowed_collision_matrix.entry_values[j].enabled[k];
    				}
    			}
    		}
    	}
    }
    
    
    void Moveit_mediator::task_planner(){
    	/* There are 2 ways to interprete a Task 
    	1. A box position in acm is also the first entry in task, 
    	2. A box position in acm is not the first entry in task, in that case we can might try for each position
    	*/
    	auto jq = job_reader_->robot_data();
    	auto cd = cuboid_reader_->cuboid_bin();
    
    	//std::vector<std::string> objs =  {"bottle1", "bottle2"};
    
    	for (int i = 0; i < cd.size(); i ++){
    		std::stringstream ss;
    		ss << "box_" << i;
    
    		moveit_msgs::CollisionObject item;
    		item.id = ss.str();
    		item.header.frame_id = "world";
    		item.header.stamp = ros::Time::now();
    		item.primitives.resize(1);
    		item.primitives[0].type = item.primitives[0].BOX;
    		item.primitives[0].dimensions.resize(3);
    		item.primitives[0].dimensions[0] = cd[i].x_depth;
    		item.primitives[0].dimensions[1] = cd[i].y_width;
    		item.primitives[0].dimensions[2] = cd[i].z_heigth;
    
    		item.primitive_poses.resize(1);
    		item.primitive_poses[0].position.x = cd[i].Pose.position.x;
    		item.primitive_poses[0].position.y = cd[i].Pose.position.y;
    		item.primitive_poses[0].position.z = cd[i].Pose.position.z;
    		item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x;
    		item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
    		item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
    		item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
    		item.operation = item.ADD;
    
    		psi_->applyCollisionObject(item);
    		acm_.insert(std::pair<std::string, std::vector<uint8_t>>(item.id, std::vector<uint8_t>()));
    
    		// Could also safe id's somewhere
    	}
    	
    	// Setup shared ACM
    	for(auto& a: acm_) a.second.resize(acm_.size(), 0);
    
    	std::regex item("box_([0-9]+)");
        std::smatch match;
    
    	// ----------------------------------
    	// GROOT Strategie => erst FROM TEXT
    	// ----------------------------------
    
    	BT::BehaviorTreeFactory factory;
    	factory.registerNodeType<Planning_scene>("PS");
    	factory.registerNodeType<Trajetory>("Trajetory");
    	factory.registerNodeType<Execution>("Execution");
    	factory.registerNodeType<Position_condition>("Position_condition");
    	factory.registerNodeType<Parallel_robot>("Parallel_robot");
    
    	std::map<const std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>> task_but_different;
    
    	while(!jq.empty()){
    		for(auto& s_co : psi_->getObjects()){
    			if(!std::regex_match(s_co.first, match, item)) continue;
    			
    			std::pair<std::string, job_data> temp = jq.front();
    			ROS_INFO("1. job entry %f %f %f", temp.second.jobs_.front().getOrigin().getX(), temp.second.jobs_.front().getOrigin().getY(), temp.second.jobs_.front().getOrigin().getZ());
    			ROS_INFO("object position %f %f %f", s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z);
    			if(tf2::tf2Distance2(temp.second.jobs_.front().getOrigin(), tf2::Vector3(s_co.second.pose.position.x, s_co.second.pose.position.y, s_co.second.pose.position.z )) == 0) {
    				std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> bt_list;
    				
    				Moveit_robot* mr;
    				for(auto* r: robots_) if (r->name() == temp.first) mr = dynamic_cast<Moveit_robot*>(r);
    
    				// loop jobs
    				for (int k = 1; k < temp.second.jobs_.size(); k++){
    					moveit::task_constructor::Task mgt = create_Task(mr, psi_->getObjects().at(s_co.first), temp.second.jobs_[k]);
    					if(mgt.plan(1)) {
    						moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
    						mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
    						bt_list.push_back(e);
    
    						moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
    						temp_co.operation = temp_co.MOVE;
    						temp_co.pose.position.x = temp.second.jobs_[k].getOrigin().getX();
    						temp_co.pose.position.y = temp.second.jobs_[k].getOrigin().getY();
    						temp_co.pose.position.z = temp.second.jobs_[k].getOrigin().getZ();
    						temp_co.pose.orientation.x = temp.second.jobs_[k].getRotation().getX();
    						temp_co.pose.orientation.y = temp.second.jobs_[k].getRotation().getY();
    						temp_co.pose.orientation.z = temp.second.jobs_[k].getRotation().getZ();
    						temp_co.pose.orientation.w = temp.second.jobs_[k].getRotation().getW();
    						psi_->applyCollisionObject(temp_co);
    					}
    				}
    
    				/*
    				if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("om");condition->init(s_co.first, temp.second.jobs_.front().getOrigin(), psi_.get()); ++node_it;}
    
    				for(auto& ets : bt_list)
    					if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("tr");execution->init(planning_scene_diff_publisher_.get(), mr->mgi().get(), ets); ++node_it;}
    				*/
    
    				for (int i = 0; i < cd.size(); i ++){
    					std::stringstream ss;
    					ss << "box_" << i;
    
    					moveit_msgs::CollisionObject item;
    					item.id = ss.str();
    					item.header.frame_id = "world";
    					item.header.stamp = ros::Time::now();
    					item.primitives.resize(1);
    					item.primitives[0].type = item.primitives[0].BOX;
    					item.primitives[0].dimensions.resize(3);
    					item.primitives[0].dimensions[0] = cd[i].x_depth;
    					item.primitives[0].dimensions[1] = cd[i].y_width;
    					item.primitives[0].dimensions[2] = cd[i].z_heigth;
    
    					item.pose.position.x = cd[i].Pose.position.x;
    					item.pose.position.y = cd[i].Pose.position.y;
    					item.pose.position.z = cd[i].Pose.position.z;
    					item.pose.orientation.x = cd[i].Pose.orientation.x;
    					item.pose.orientation.y = cd[i].Pose.orientation.y;
    					item.pose.orientation.z = cd[i].Pose.orientation.z;
    					item.pose.orientation.w = cd[i].Pose.orientation.w;
    					item.operation = item.MOVE;
    
    					psi_->applyCollisionObject(item);
    					// Could also safe id's somewhere
    				}
    
    				
    				try{
    					task_but_different.at(mr->name()).push_back(std::tuple<std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list));
    				} catch(std::out_of_range &oor) {
    					std::tuple<const std::string&, tf2::Vector3&, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>&> tuple(s_co.first, temp.second.jobs_.front().getOrigin(), bt_list);
    					task_but_different.insert(std::pair<std::string, std::vector<std::tuple<const std::string, tf2::Vector3, std::vector<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>>(mr->name(),{tuple}));
    				}
    				jq.pop_front();
    			} else {jq.push_back(temp);}
    		}
    	}
    
    	std::stringstream ss;
    	ss << "<root main_tree_to_execute = \"MainTree\">\n";
    	ss << "<BehaviorTree ID=\"MainTree\">\n";
    	ss << "<Control ID=\"Parallel\" name=\"Agents\" success_threshold=\""<< robots_.size() << "\" failure_threshold=\""<< robots_.size() << "\">\n";
    
    	for(int i =0; i < robots_.size();i++){
    		try{
    			ss << "<Control ID=\"Parallel_robot\" name=\""<< robots_[i]->name() << "\" success_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\" failure_threshold=\""<< task_but_different.at(robots_[i]->name()).size() << "\">\n";
    			for (auto& p_obj: task_but_different.at(robots_[i]->name())){
    				ss << "<SequenceStar name=\"root_sequence\">\n";
    				ss << "<Condition ID=\"Position_condition\" name=\"Position_condition\"/>\n";
    				for(int j = 0; j < std::get<2>(p_obj).size(); j++){
    					ss << "<Action ID=\"Execution\" name=\"Execution\"/>\n";
    				}
    				ss << "</SequenceStar>\n";
    			}
    			ss << "</Control>\n";
    		} catch(std::out_of_range& oor){}
    	}
    	ss << "</Control>\n";
    	ss << "</BehaviorTree>\n";
    	ss << "</root>\n";
    
    	std::cout << ss.str();
    	auto tree = factory.createTreeFromText(ss.str());
    	auto node_it = tree.nodes.begin();
    
    	
    
    	for(int i =0; i < robots_.size();i++){
    		try{
    			for (auto& p_obj: task_but_different.at(robots_[i]->name())){
    				while (node_it->get()->type() == BT::NodeType::CONTROL) ++node_it;
    				if(node_it->get()->type() == BT::NodeType::ACTION) ROS_INFO("Action");
    				if(node_it->get()->type() == BT::NodeType::CONDITION) ROS_INFO("Condition");
    				if(node_it->get()->type() == BT::NodeType::CONTROL) ROS_INFO("Control");
    				if(node_it->get()->type() == BT::NodeType::DECORATOR) ROS_INFO("Decorator");
    
    				if(auto condition = dynamic_cast<Position_condition*>(node_it->get())) {ROS_INFO("init Condition");condition->init(std::get<0>(p_obj), std::get<1>(p_obj), psi_.get());++node_it;}
    				for(int j = 0; j < std::get<2>(p_obj).size(); j++){
    					auto mr = dynamic_cast<Moveit_robot*>(robots_[i]);
    					if(auto execution = dynamic_cast<Execution*>(node_it->get())) {ROS_INFO("init Action");execution->init(&executions_, planning_scene_diff_publisher_.get(), mr, std::get<2>(p_obj)[j]);++node_it;}
    				}
    				ss << "</SequenceStar>\n";
    			}
    			ss << "</Control>\n";
    		} catch(std::out_of_range& oor){}
    	}
    
    
    	BT::PublisherZMQ zmq(tree);
    	ros::Duration sleep(1);
    	BT::NodeStatus status = BT::NodeStatus::RUNNING;
    	while( status == BT::NodeStatus::RUNNING){
    		status = tree.tickRoot();
    
    		std::vector<std::thread> th;
    		moveit_msgs::PlanningScene ps_m;
    		
    		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]);
    					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);
    		 		}
    			}
    		}
    
    		merge_acm(ps_m);
    		planning_scene_diff_publisher_->publish(ps_m);
    	}
    
    
    	// clean up
    	for (int i = 0; i < cd.size(); i ++){
    		std::stringstream ss;
    		ss << "box_" << i;
    
    		moveit_msgs::CollisionObject item;
    		item.id = ss.str();
    		item.header.frame_id = "world";
    		item.header.stamp = ros::Time::now();
    		item.primitives.resize(1);
    		item.primitives[0].type = item.primitives[0].BOX;
    		item.primitives[0].dimensions.resize(3);
    		item.primitives[0].dimensions[0] = cd[i].x_depth;
    		item.primitives[0].dimensions[1] = cd[i].y_width;
    		item.primitives[0].dimensions[2] = cd[i].z_heigth;
    
    		item.pose.position.x = cd[i].Pose.position.x;
    		item.pose.position.y = cd[i].Pose.position.y;
    		item.pose.position.z = cd[i].Pose.position.z;
    		item.pose.orientation.x = cd[i].Pose.orientation.x;
    		item.pose.orientation.y = cd[i].Pose.orientation.y;
    		item.pose.orientation.z = cd[i].Pose.orientation.z;
    		item.pose.orientation.w = cd[i].Pose.orientation.w;
    		item.operation = item.MOVE;
    
    		psi_->applyCollisionObject(item);
    		// Could also safe id's somewhere
    	}
    	
    }
    
    
    void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
    	moveit_msgs::PlanningScene::_allowed_collision_matrix_type acmt;
    	acmt.entry_values.resize(acm_.size());
    
    	int i = 0;
    	for (auto& a : acm_){
    		acmt.entry_names.push_back(a.first);
    		acmt.entry_values[i].enabled = a.second;
    		i++;
    	}
    
    	ps_m.allowed_collision_matrix = acmt;
    }
    
    void Moveit_mediator::status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status){
     if (!status->status_list.empty()){
    	ROS_INFO("%i", status->status_list.size());
       }
    }
    
    void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr){
    	// get full mr link list 
    	std::vector<std::string> links;
    	for (auto link : mr->mgi()->getLinkNames())links.push_back(link);
    	links.push_back(mr->map()["left_finger"]);
    	links.push_back(mr->map()["right_finger"]);
    	links.push_back(mr->map()["hand_link"]);
    	links.push_back(mr->map()["base"]);
    
    
    	for (auto ao : in.robot_state.attached_collision_objects) out.robot_state.attached_collision_objects.push_back(ao);
    	if (in.robot_state.is_diff) out.robot_state.is_diff = in.robot_state.is_diff;
    	if (in.is_diff) out.is_diff = in.is_diff;
    	out.robot_state.joint_state.header = in.robot_state.joint_state.header;
    	out.robot_model_name = "panda";
    
    
    
    	for (auto link : links) {
    		for(int i = 0; i < in.robot_state.joint_state.name.size(); i++){
    			if(link.c_str() == in.robot_state.joint_state.name[i].c_str()){
    				out.robot_state.joint_state.effort.push_back(in.robot_state.joint_state.effort[i]);
    				out.robot_state.joint_state.position.push_back(in.robot_state.joint_state.position[i]);
    				out.robot_state.joint_state.velocity.push_back(in.robot_state.joint_state.velocity[i]);
    				ROS_INFO("check");
    			}
    		}
    
    		for(int i = 0; i < in.link_padding.size(); i++){
    			if(link.c_str() == in.link_padding[i].link_name.c_str()){
    				out.link_padding.push_back(in.link_padding[i]);
    			}
    		}
    
    		for(int i = 0; i < in.link_scale.size(); i++){
    			if(link.c_str() == in.link_scale[i].link_name.c_str()){
    				out.link_scale.push_back(in.link_scale[i]);
    			}
    		}
    	}
    	
    }
    
    
    void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){
    	mr.mgi()->execute(rt);
    }
    
    
    
    
    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";
    
    
        for (Abstract_robot* ar : robots_){
            std::string str;
            if(ar->check_single_object_collision(t, str)) support_surface1 = str;
            if(ar->check_single_object_collision(target, str)) support_surface2= str;
        }
    
    	ROS_INFO("ss1 %s", support_surface1.c_str());
    	ROS_INFO("ss2 %s", support_surface2.c_str());
    
    
    	const std::string object = source.id;
        moveit::task_constructor::Task task_;
    
    	std::string name = "Pick&Place";
    	task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec())));
    	task_.loadRobotModel();
    	task_.setRobotModel(mr->mgi()->getRobotModel());
    
    	// Set task properties
    	task_.setProperty("group", mr->name());
    	task_.setProperty("eef", mr->map()["eef_name"]);
    	task_.setProperty("hand", mr->map()["hand_group_name"]);
    	task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]);
    	task_.setProperty("ik_frame", mr->map()["hand_frame"]);
    
    	moveit::task_constructor::Stage* current_state_ptr = nullptr;  
    	{
    		auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state");
    		auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state));
    		applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) {
    			if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
    				comment = "object with id '" + object + "' is already attached and cannot be picked";
    				return false;
    			}
    			return true;
    		});
    
    		current_state_ptr = applicability_filter.get();
    		task_.add(std::move(applicability_filter));
    	}
    
    	{  // Open Hand
    		auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
    		stage->setGroup(mr->map()["eef_name"]);
    		stage->setGoal("open");
    		task_.add(std::move(stage));
    	}
    
    	{  // Move-to pre-grasp
    		auto stage = std::make_unique< moveit::task_constructor::stages::Connect>(
    		    "move to pick",  moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(),  sampling_planner_} });
    		stage->setTimeout(5.0);
    		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
    		task_.add(std::move(stage));
    	}
    
    	 moveit::task_constructor::Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator
    	{
    		auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object");
    		task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
    		grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
    
    
    		{	// Approach Obj
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_);
    			stage->properties().set("marker_ns", "approach_object");
    			stage->properties().set("link", mr->map()["hand_frame"]);
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
    			stage->setMinMaxDistance(0.07, 0.2);
    
    			// Set hand forward direction
    			geometry_msgs::Vector3Stamped vec;
    			vec.header.frame_id = mr->map()["hand_frame"];
    			vec.vector.z = 1.0;
    			stage->setDirection(vec);
    			grasp->insert(std::move(stage));
    		}
    
    		{
    			// Sample grasp pose
    			auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose");
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
    			stage->properties().set("marker_ns", "grasp_pose");
    			stage->setPreGraspPose("open");
    			stage->setObject(object);
    			stage->setAngleDelta(M_PI / 12);
    			stage->setMonitoredStage(current_state_ptr);  // Hook into current state
    
    			// Compute IK
    			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
                                       Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
                                       Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
    			Eigen::Translation3d trans(0.1f, 0, 0);
    			Eigen::Isometry3d ik = eigen * trans;
    
    			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage));
    			wrapper->setMaxIKSolutions(8);
    			wrapper->setMinSolutionDistance(1.0);
    			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
    			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
    			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
    			grasp->insert(std::move(wrapper));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
    			stage->allowCollisions(
    			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
    			    true);
    			grasp->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
    			stage->setGroup(mr->map()["eef_name"]);
    			stage->setGoal("close");
    			grasp->insert(std::move(stage));
    		}
    
    		{	// Attach obj
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
    			stage->attachObject(object, mr->map()["hand_frame"]);
    			attach_object_stage = stage.get();
    			grasp->insert(std::move(stage));
    		}
    
    		{	// Allow Collision obj table
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)");
    			stage->allowCollisions({ object }, support_surface1, true);
    			grasp->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_);
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
    			stage->setMinMaxDistance(0.1, 0.2);
    			stage->setIKFrame(mr->map()["hand_frame"]);
    			stage->properties().set("marker_ns", "lift_object");
    
    			// Set upward direction
    			geometry_msgs::Vector3Stamped vec;
    			vec.header.frame_id = "world";
    			vec.vector.z = 1.0;
    			stage->setDirection(vec);
    			grasp->insert(std::move(stage));
    		}
    
    		{	// forbid collision
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)");
    			stage->allowCollisions({ object }, support_surface1, false);
    			grasp->insert(std::move(stage));
    		}
    
    		// Add grasp container to task
    		task_.add(std::move(grasp));
    	}
    
    	{
    		auto stage = std::make_unique<moveit::task_constructor::stages::Connect>(
    		    "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } });
    		stage->setTimeout(5.0);
    		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT);
    		task_.add(std::move(stage));
    	}
    
    	{
    		auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object");
    		task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
    		place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" });
    
    		{
    		auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)");
    		stage->allowCollisions( {object} , support_surface2, true);
    		place->insert(std::move(stage));
    		}
    
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_);
    			stage->properties().set("marker_ns", "lower_object");
    			stage->properties().set("link", mr->map()["hand_frame"]);
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
    			stage->setMinMaxDistance(.03, .13);
    
    			// Set downward direction
    			geometry_msgs::Vector3Stamped vec;
    			vec.header.frame_id = "world";
    			vec.vector.z = -1.0;
    			stage->setDirection(vec);
    			place->insert(std::move(stage));
    		}
    
    		{
    			// Generate Place Pose
    			auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose");
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" });
    			stage->properties().set("marker_ns", "place_pose");
    			stage->setObject(object);
    
    			// Set target pose
    			geometry_msgs::PoseStamped p;
    			p.header.frame_id = "world";
    			p.pose.position.x = target.getOrigin().getX();
    			p.pose.position.y = target.getOrigin().getY();
    			p.pose.position.z = target.getOrigin().getZ();
    			p.pose.orientation.x = target.getRotation().getX();
    			p.pose.orientation.y = target.getRotation().getY();
    			p.pose.orientation.z = target.getRotation().getZ();
    			p.pose.orientation.w = target.getRotation().getW();
    
    
    
    			stage->setPose(p);
    			stage->setMonitoredStage(attach_object_stage);  // Hook into attach_object_stage
    
    			// Compute IK
    			Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) *
                                       Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) *
                                       Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ());
    			Eigen::Translation3d trans(0.1f, 0, 0);
    			Eigen::Isometry3d ik = eigen * trans;
    			auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage));
    			wrapper->setMaxIKSolutions(2);
    			wrapper->setIKFrame(ik, mr->map()["hand_frame"]);
    			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" });
    			wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" });
    			place->insert(std::move(wrapper));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_);
    			stage->setGroup(mr->map()["eef_name"]);
    			stage->setGoal("open");
    			place->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)");
    			stage->allowCollisions(
    			    object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(),
    			    false);
    			place->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object");
    			stage->detachObject(object, mr->map()["hand_frame"]);
    			place->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_);
    			stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
    			stage->setMinMaxDistance(.1, .2);
    			stage->setIKFrame(mr->map()["hand_frame"]);
    			stage->properties().set("marker_ns", "retreat");
    			geometry_msgs::Vector3Stamped vec;
    			vec.header.frame_id = mr->map()["hand_frame"];
    			vec.vector.z = -1.0;
    			stage->setDirection(vec);
    			place->insert(std::move(stage));
    		}
    
    		{
    			auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_);
    			stage->setGroup(mr->map()["eef_name"]);
    			stage->setGoal("close");
    			place->insert(std::move(stage));
    		}
    
    
    		// Add place container to task
    		task_.add(std::move(place));
    	}
    
    	{
    		auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_);
    		stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" });
    		stage->setGoal("ready");
    		stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD);
    		task_.add(std::move(stage));
    	}
    
    	return task_;
    }
    
    
    Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, std::shared_ptr<ros::NodeHandle> const& nh)
        : Abstract_mediator(objects, pub), nh_(nh)
        , sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>())
        , cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>())
        , psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>())
        , mgi_(std::make_shared<moveit::planning_interface::MoveGroupInterface>("panda_arms"))
        , planning_scene_diff_publisher_(std::make_shared<ros::Publisher>(nh_->advertise<moveit_msgs::PlanningScene>("planning_scene", 1)))
        , planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
    	, job_reader_(std::make_unique<Job_reader>(nh_))
        , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
    
            if (planning_scene_monitor_->getPlanningScene()){
                planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
                                                                  "planning_scene");
                planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
            } else {
                ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
                return;
            }
    
            robot_model_loader::RobotModelLoaderPtr robot_model_loader;
            robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
            robot_model_ = robot_model_loader->getModel();
            ps_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
    
            // planner 
            sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
    
            // cartesian
    	    cartesian_planner_->setMaxVelocityScaling(1.0);
    	    cartesian_planner_->setMaxAccelerationScaling(1.0);
    	    cartesian_planner_->setStepSize(.01);
    
        };