diff --git a/.vscode/settings.json b/.vscode/settings.json
index 401dd611113a87620488af152e225b613cb80334..1de818d0562b9e77025cae6af75d7bd8ffe63c48 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -74,6 +74,7 @@
         "cinttypes": "cpp",
         "typeindex": "cpp",
         "typeinfo": "cpp",
-        "variant": "cpp"
+        "variant": "cpp",
+        "future": "cpp"
     }
 }
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f2afac981d4baa36d68f7c9ab7ae2ba178d26869..3360be4aa23d40747c6ee4350b3c91f1a03e91ca 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,8 +1,11 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(multi_cell_builder)
 # C++ 11
 add_compile_options(-std=c++17)
 
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 # Load catkin and all dependencies required for this package
 find_package(catkin REQUIRED COMPONENTS
   moveit_task_constructor_core
@@ -24,9 +27,11 @@ find_package(catkin REQUIRED COMPONENTS
   tf_conversions
   trajectory_msgs
   pcl_ros
+  actionlib_msgs
 
   gb_grasp
   moveit_grasps
+  behaviortree_cpp_v3
 )
 
 find_package(yaml-cpp REQUIRED)
@@ -83,7 +88,11 @@ src/reader/wing_reader.cpp
 src/reader/cuboid_reader.cpp
 src/reader/job_reader.cpp
 
-
+src/bt/trajetory.cpp
+src/bt/planning_scene.cpp
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
 
 
 )
@@ -106,6 +115,13 @@ src/reader/wing_reader.cpp
 src/reader/cuboid_reader.cpp
 src/reader/job_reader.cpp
 
+src/bt/trajetory.cpp
+src/bt/planning_scene.cpp
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+
+
 )
 
 add_executable(grasp_cell_routine src/grasp_cell_routine.cpp
@@ -127,6 +143,14 @@ src/reader/wing_reader.cpp
 src/reader/cuboid_reader.cpp
 src/reader/job_reader.cpp
 
+
+src/bt/trajetory.cpp
+src/bt/planning_scene.cpp
+src/bt/execution.cpp
+src/bt/position_condition.cpp
+src/bt/parallel_robot.cpp
+
+
 )
 
 
@@ -150,6 +174,7 @@ target_link_libraries(base_routine
   ${OCTOMAP_LIBRARIES}
   ${PCL_LIBRARY_DIRS}
   ${moveit_grasps_LIBRARIES}
+  ${BEHAVIOR_TREE_LIBRARY}
   yaml-cpp
 )
 target_link_libraries(cell_routine 
@@ -157,7 +182,9 @@ target_link_libraries(cell_routine
   ${OCTOMAP_LIBRARIES}
   ${PCL_LIBRARY_DIRS}
   ${moveit_grasps_LIBRARIES}
+  ${BEHAVIOR_TREE_LIBRARY}
   yaml-cpp
+
 )
 
 target_link_libraries(grasp_cell_routine 
@@ -165,6 +192,7 @@ target_link_libraries(grasp_cell_routine
   ${OCTOMAP_LIBRARIES}
   ${PCL_LIBRARY_DIRS}
   ${moveit_grasps_LIBRARIES}
+  ${BEHAVIOR_TREE_LIBRARY}
   yaml-cpp
 )
 
diff --git a/include/bt/execution.h b/include/bt/execution.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef31a31c007360c5c084332192d3d2600ebe550e
--- /dev/null
+++ b/include/bt/execution.h
@@ -0,0 +1,43 @@
+#ifndef EXECUTION_
+#define EXECUTION_
+
+#include "ros/ros.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/tree_node.h"
+#include "behaviortree_cpp_v3/basic_types.h"
+#include "behaviortree_cpp_v3/leaf_node.h"
+#include "impl/abstract_mediator.h"
+#include "impl/moveit_mediator.h"
+
+#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+class Execution : public BT::StatefulActionNode{
+    private:
+    moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_;
+    Moveit_robot* mr_reference_;
+    ros::Publisher* publisher_reference_;
+    Moveit_mediator* mediator_reference_;
+    std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_;
+
+
+    public:
+    Execution(const std::string& name, const BT::NodeConfiguration& config);
+    void init(Moveit_mediator* mediator_reference, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
+
+    inline static BT::PortsList providedPorts() { return {}; }
+
+    BT::NodeStatus onStart() override;
+    BT::NodeStatus onRunning() override;
+    void onHalted() override;
+};
+#endif
+
+
+
diff --git a/include/bt/parallel_robot.h b/include/bt/parallel_robot.h
new file mode 100644
index 0000000000000000000000000000000000000000..21a51bfab17f73a141495be65d6e333a1d0b6f3d
--- /dev/null
+++ b/include/bt/parallel_robot.h
@@ -0,0 +1,46 @@
+#ifndef PARALLEL_ROBOT_
+#define PARALLEL_ROBOT_
+
+#pragma once
+
+#include <set>
+#include "behaviortree_cpp_v3/control_node.h"
+
+class Parallel_robot : public BT::ControlNode{
+public:
+  Parallel_robot(const std::string& name, int success_threshold, int failure_threshold = 1);
+  Parallel_robot(const std::string& name, const BT::NodeConfiguration& config);
+
+  static BT::PortsList providedPorts(){
+    return {BT::InputPort<int>(THRESHOLD_SUCCESS, "number of childen which need to succeed "
+                                              "to "
+                                              "trigger a SUCCESS"),
+            BT::InputPort<int>(THRESHOLD_FAILURE, 1,
+                           "number of childen which need to fail to trigger a FAILURE")};
+  }
+
+  ~Parallel_robot() override = default;
+
+  virtual void halt() override;
+
+  size_t successThreshold() const;
+  size_t failureThreshold() const;
+  void setSuccessThreshold(int threshold_M);
+  void setFailureThreshold(int threshold_M);
+
+private:
+  int success_threshold_;
+  int failure_threshold_;
+
+  std::set<int> skip_list_;
+
+  bool read_parameter_from_ports_;
+  static constexpr const char* THRESHOLD_SUCCESS = "success_threshold";
+  static constexpr const char* THRESHOLD_FAILURE = "failure_threshold";
+
+  virtual BT::NodeStatus tick() override;
+};
+
+#endif
+
+
diff --git a/include/bt/planning_scene.h b/include/bt/planning_scene.h
new file mode 100644
index 0000000000000000000000000000000000000000..25cf44d7f1bdb7132733f6a25dbc76ba4514eacb
--- /dev/null
+++ b/include/bt/planning_scene.h
@@ -0,0 +1,30 @@
+#ifndef PLANNING_SCENE_
+#define PLANNING_SCENE_
+
+#include "ros/ros.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/tree_node.h"
+#include "behaviortree_cpp_v3/basic_types.h"
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+class Planning_scene : public BT::AsyncActionNode{
+    private:
+    ros::Publisher* publisher_reference_;
+    moveit_msgs::PlanningScene planning_scene_;
+
+    public:
+    Planning_scene(const std::string& name, const BT::NodeConfiguration& config);
+    void init(ros::Publisher* publisher_reference, moveit_msgs::PlanningScene& planning_scene);
+
+
+    // You must override the virtual function tick()
+    // this example doesn't require any port
+    inline static BT::PortsList providedPorts() { return {}; }
+    BT::NodeStatus tick() override;
+};
+#endif
\ No newline at end of file
diff --git a/include/bt/position_condition.h b/include/bt/position_condition.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef66a618b399d2412415a12a7866d51ccf325f59
--- /dev/null
+++ b/include/bt/position_condition.h
@@ -0,0 +1,36 @@
+#ifndef POSITION_CONDITION_
+#define POSITION_CONDITION_
+
+#include "ros/ros.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/tree_node.h"
+#include "behaviortree_cpp_v3/basic_types.h"
+#include "behaviortree_cpp_v3/leaf_node.h"
+
+#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+
+class Position_condition : public BT::ConditionNode {
+public:
+Position_condition(const std::string& name, const BT::NodeConfiguration& config);
+
+void init(const std::string& obj_name,tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi);
+
+inline static BT::PortsList providedPorts() { return {}; }
+
+protected:
+
+virtual BT::NodeStatus tick() override;
+std::string obj_name_;
+moveit::planning_interface::PlanningSceneInterface* psi_;
+tf2::Vector3 start_pos_;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bt/trajetory.h b/include/bt/trajetory.h
new file mode 100644
index 0000000000000000000000000000000000000000..54d4f99229aaf0b61f0d8eb9dd8bc594c02a2951
--- /dev/null
+++ b/include/bt/trajetory.h
@@ -0,0 +1,32 @@
+#ifndef TRAJETORY_
+#define TRAJETORY_
+
+#include "ros/ros.h"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/tree_node.h"
+#include "behaviortree_cpp_v3/basic_types.h"
+
+
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+class Trajetory : public BT::AsyncActionNode{
+    private:
+    moveit::planning_interface::MoveGroupInterface* mgi_reference_;
+    moveit_msgs::RobotTrajectory trajetory_;
+
+    public:
+    Trajetory(const std::string& name, const BT::NodeConfiguration& config);
+    void init (moveit::planning_interface::MoveGroupInterface* mgi_reference, moveit_msgs::RobotTrajectory& trajetory);
+
+
+
+    // You must override the virtual function tick()
+    inline static BT::PortsList providedPorts() { return {}; }
+    BT::NodeStatus tick() override;
+};
+#endif
\ No newline at end of file
diff --git a/include/impl/moveit_mediator.h b/include/impl/moveit_mediator.h
index d9866f62d8e50a6e97e044a9076767e873c782a6..b1d24493baf421f9f14dfcc8abd45c377ebfcfc3 100644
--- a/include/impl/moveit_mediator.h
+++ b/include/impl/moveit_mediator.h
@@ -47,6 +47,8 @@
 #include <moveit/moveit_cpp/moveit_cpp.h>
 #include "reader/job_reader.h"
 #include "reader/cuboid_reader.h"
+#include <thread>
+
 
 #include <stdint.h>
 
@@ -78,6 +80,7 @@ class Moveit_mediator : public Abstract_mediator{
 
 
 
+    std::vector<std::thread*> executions_;
 
 
     public:
@@ -85,13 +88,14 @@ class Moveit_mediator : public Abstract_mediator{
 
     void setup_task();
     inline std::shared_ptr<moveit::planning_interface::MoveGroupInterface> mgi() {return mgi_;};
-
+    inline std::vector<std::thread*>& executions(){return executions_;};
 
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wing,int& robot) override;
     void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
     void connect_robots(Abstract_robot* robot) override;
+    void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status);
 
 
     void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
@@ -100,7 +104,10 @@ class Moveit_mediator : public Abstract_mediator{
     void task_planner();
     void publish_tables();
     void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
-    void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps);
+
+    // void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt);
+    void parallel_exec();
+
     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_;};
     inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
diff --git a/launch/cell_routine.launch b/launch/cell_routine.launch
index 373d6c3c27d1e7d073dee7ae2295b564ad27ec17..64ed8c3725cbaf8bd23dbaff4e4267a70feeb9d1 100644
--- a/launch/cell_routine.launch
+++ b/launch/cell_routine.launch
@@ -27,6 +27,9 @@
         <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
     </include>
 
+    <node pkg="groot" type="Groot" name="Groot" output="screen" required="true">
+    </node>
+
     <node pkg="multi_cell_builder" type="cell_routine" name="cell_routine" output="screen" required="true">
     </node>
 </launch>
diff --git a/package.xml b/package.xml
index 80f22b71a8119d9f7451eae5ac034c728e030c3a..4e3f59cb597edeedb4e5ad6e9c48138441c6969c 100644
--- a/package.xml
+++ b/package.xml
@@ -32,6 +32,8 @@
   <build_depend>moveit_task_constructor_visualization</build_depend>
   <build_depend>pcl_ros</build_depend>
   <build_depend>moveit_task_constructor_core</build_depend>
+  <build_depend>behaviortree_cpp_v3</build_depend>
+
 
 
   <build_depend>moveit_task_constructor_capabilities</build_depend>
@@ -42,7 +44,7 @@
   <run_depend>moveit_grasps</run_depend>
   <run_depend>moveit_grasps_filter</run_depend>
 
-
+  <run_depend>behaviortree_cpp_v3</run_depend>
   <run_depend>moveit_ros_planning_interface</run_depend>
   <run_depend>filesystem</run_depend> 
   <run_depend>pcl_ros</run_depend>
diff --git a/src/bt/execution.cpp b/src/bt/execution.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b64e7fcb98f6120a6b849205d7ee5b4249ac53e
--- /dev/null
+++ b/src/bt/execution.cpp
@@ -0,0 +1,77 @@
+#include "bt/execution.h"
+
+Execution::Execution(const std::string& name, const BT::NodeConfiguration& config)
+    : BT::StatefulActionNode(name, config) {}
+
+
+/*
+BT::NodeStatus Execution::tick() {
+    ROS_INFO("davor");
+    if (it_ != ets_.solution.sub_trajectory.end()){
+        mgi_reference_->execute(it_->trajectory);
+        publisher_reference_->publish(it_->scene_diff);
+        it_++;
+        ROS_INFO("true");
+        return BT::NodeStatus::RUNNING;
+    } else {
+        ROS_INFO("false");
+        return BT::NodeStatus::SUCCESS;
+    }
+};
+*/
+
+BT::NodeStatus Execution::onStart(){
+    if (it_ != ets_.solution.sub_trajectory.end()){
+        //mr_reference_->execute(it_->trajectory);
+        ROS_INFO("start");
+        // std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_);
+        // if(t.joinable())t.join();
+        // mediator_reference_->parallel_exec();
+        // mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_, std::ref(*mr_reference_), it_->trajectory));
+        mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_));
+        for (int i = 0; i < mediator_reference_->executions().size();i++){
+            if(mediator_reference_->executions()[i]->joinable()) mediator_reference_->executions()[i]->join();
+        }
+
+        //publisher_reference_->publish(it_->scene_diff);
+        ROS_INFO("%i", mediator_reference_->executions().size());
+        ROS_INFO("after join in start");
+
+        it_++;
+        return BT::NodeStatus::RUNNING;
+    } else {
+        return BT::NodeStatus::SUCCESS;
+    }
+ }
+
+    // If onStart() returned RUNNING, we will keep calling
+    // this method until it return something different from RUNNING
+BT::NodeStatus Execution::onRunning(){
+    if (it_ != ets_.solution.sub_trajectory.end()){
+        //mgi_reference_->execute(it_->trajectory);
+        //mediator_reference_->parallel_exec();
+        ROS_INFO("running");
+        //std::thread t(&Moveit_mediator::parallel_exec, mediator_reference_);
+        // if(t.joinable())t.join();
+        mediator_reference_->executions().push_back(new std::thread(&Moveit_mediator::parallel_exec, mediator_reference_));
+        ROS_INFO("after join in running");
+        //mediator_reference_->executions().push_back(std::thread(&Moveit_mediator::parallel_exec, mediator_reference_));
+
+        //publisher_reference_->publish(it_->scene_diff);
+        it_++;
+        return BT::NodeStatus::RUNNING;
+    } else {
+        return BT::NodeStatus::SUCCESS;
+    }
+}
+
+    // callback to execute if the action was aborted by another node
+void Execution::onHalted(){}
+
+void Execution::init(Moveit_mediator* mediator_reference ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
+    mr_reference_= mr_reference;
+    publisher_reference_ = publisher_reference;
+    ets_ = ets;
+    it_ = ets_.solution.sub_trajectory.begin();
+    mediator_reference_ = mediator_reference_;
+}
\ No newline at end of file
diff --git a/src/bt/parallel_robot.cpp b/src/bt/parallel_robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e43274b63f68f322989686afc230aaca93bca1d
--- /dev/null
+++ b/src/bt/parallel_robot.cpp
@@ -0,0 +1,145 @@
+#include "bt/parallel_robot.h"
+
+/* Copyright (C) 2015-2018 Michele Colledanchise -  All Rights Reserved
+ * Copyright (C) 2018-2020 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <algorithm>
+#include <cstddef>
+
+#include "behaviortree_cpp_v3/controls/parallel_node.h"
+
+constexpr const char* BT::ParallelNode::THRESHOLD_FAILURE;
+constexpr const char* BT::ParallelNode::THRESHOLD_SUCCESS;
+
+Parallel_robot::Parallel_robot(const std::string& name, int success_threshold, int failure_threshold) :
+  BT::ControlNode(name, {}),
+  success_threshold_(success_threshold),
+  failure_threshold_(failure_threshold),
+  read_parameter_from_ports_(false){
+  setRegistrationID("Parallel_robot");
+}
+
+Parallel_robot::Parallel_robot(const std::string& name, const BT::NodeConfiguration& config) :
+  BT::ControlNode(name, config),
+  success_threshold_(1),
+  failure_threshold_(1),
+  read_parameter_from_ports_(true)
+{}
+
+BT::NodeStatus Parallel_robot::tick(){
+  if (read_parameter_from_ports_){
+    if (!getInput(THRESHOLD_SUCCESS, success_threshold_)){
+      throw BT::RuntimeError("Missing parameter [", THRESHOLD_SUCCESS, "] in ParallelNode");
+    }
+
+    if (!getInput(THRESHOLD_FAILURE, failure_threshold_)){
+      throw BT::RuntimeError("Missing parameter [", THRESHOLD_FAILURE, "] in ParallelNode");
+    }
+  }
+
+  size_t success_childred_num = 0;
+  size_t failure_childred_num = 0;
+
+  const size_t children_count = children_nodes_.size();
+
+  if (children_count < successThreshold()){
+    throw BT::LogicError("Number of children is less than threshold. Can never succeed.");
+  }
+
+  if (children_count < failureThreshold()){
+    throw BT::LogicError("Number of children is less than threshold. Can never fail.");
+  }
+
+  // Routing the tree according to the sequence node's logic:
+  for (unsigned int i = 0; i < children_count; i++){
+    TreeNode* child_node = children_nodes_[i];
+
+    bool in_skip_list = (skip_list_.count(i) != 0);
+
+    BT::NodeStatus child_status;
+    if (in_skip_list){
+      child_status = child_node->status();
+    }
+    else{
+      child_status = child_node->executeTick();
+    }
+
+    switch (child_status){
+      case BT::NodeStatus::SUCCESS: {
+        if (!in_skip_list){
+          skip_list_.insert(i);
+        }
+        success_childred_num++;
+
+        if (success_childred_num == successThreshold()){
+          skip_list_.clear();
+          BT::ControlNode::ControlNode::
+          resetChildren();
+          return BT::NodeStatus::SUCCESS;
+        }
+      }
+      break;
+
+      case BT::NodeStatus::FAILURE: {
+
+        // It fails if it is not possible to succeed anymore or if
+        // number of failures are equal to failure_threshold_
+        if ((failure_childred_num > children_count - successThreshold()) ||
+            (failure_childred_num == failureThreshold())){
+          skip_list_.clear();
+          resetChildren();
+          return BT::NodeStatus::FAILURE;
+        }
+      }
+      break;
+
+      case BT::NodeStatus::RUNNING: {
+        return BT::NodeStatus::RUNNING;
+      }
+      break;
+
+      default: {
+        throw BT::LogicError("A child node must never return IDLE");
+      }
+    }
+  }
+
+  return BT::NodeStatus::RUNNING;
+}
+
+void Parallel_robot::halt()
+{
+  skip_list_.clear();
+  ControlNode::halt();
+}
+
+size_t Parallel_robot::successThreshold() const
+{
+  return success_threshold_ < 0 ?
+             std::max(children_nodes_.size() + success_threshold_ + 1, size_t(0)) :
+             success_threshold_;
+}
+
+size_t Parallel_robot::failureThreshold() const{
+  return failure_threshold_ < 0 ?
+             std::max(children_nodes_.size() + failure_threshold_ + 1, size_t(0)) :
+             failure_threshold_;
+}
+
+void Parallel_robot::setSuccessThreshold(int threshold_M){
+  success_threshold_ = threshold_M;
+}
+
+void Parallel_robot::setFailureThreshold(int threshold_M){
+  failure_threshold_ = threshold_M;
+}
diff --git a/src/bt/planning_scene.cpp b/src/bt/planning_scene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b67f11b6e29bfdd9fcfaeabcde512cde75063d5a
--- /dev/null
+++ b/src/bt/planning_scene.cpp
@@ -0,0 +1,19 @@
+#include "bt/planning_scene.h"
+
+Planning_scene::Planning_scene(const std::string& name, const BT::NodeConfiguration& config)
+    : BT::AsyncActionNode(name, config) {}
+
+
+BT::NodeStatus Planning_scene::tick() {
+    try{
+        publisher_reference_->publish(planning_scene_);
+    } catch (ros::Exception& re) {
+        return BT::NodeStatus::FAILURE;
+    }
+    return BT::NodeStatus::SUCCESS;
+};
+
+void Planning_scene::init(ros::Publisher* publisher_reference, moveit_msgs::PlanningScene& planning_scene) {
+    publisher_reference_ = publisher_reference;
+    planning_scene_ = planning_scene;
+}
\ No newline at end of file
diff --git a/src/bt/position_condition.cpp b/src/bt/position_condition.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec6467bd19aa2b98d458af90914eff893b084690
--- /dev/null
+++ b/src/bt/position_condition.cpp
@@ -0,0 +1,24 @@
+#include "bt/position_condition.h"
+
+Position_condition::Position_condition(const std::string& name, const BT::NodeConfiguration& config)
+    : BT::ConditionNode(name, config){}
+
+void Position_condition::init(const std::string& obj_name, tf2::Vector3& start_pos, moveit::planning_interface::PlanningSceneInterface* psi){
+    obj_name_ = obj_name;
+    psi_ = psi;
+    start_pos_ = start_pos;
+}
+
+BT::NodeStatus Position_condition::tick(){
+    try {
+        auto ref_pose = psi_->getObjects().at(obj_name_);
+        if(tf2::tf2Distance2(start_pos_, tf2::Vector3(ref_pose.pose.position.x, ref_pose.pose.position.y, ref_pose.pose.position.z )) == 0){
+            return BT::NodeStatus::SUCCESS;
+        } else {
+            return BT::NodeStatus::FAILURE;
+        }
+    } catch(std::out_of_range& oor) {
+        ROS_INFO("Position to check not found");
+        return BT::NodeStatus::FAILURE;
+    }
+}
diff --git a/src/bt/trajetory.cpp b/src/bt/trajetory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6462090a103ec4ecd558d279201bca3dfdfaa370
--- /dev/null
+++ b/src/bt/trajetory.cpp
@@ -0,0 +1,14 @@
+#include "bt/trajetory.h"
+
+Trajetory::Trajetory(const std::string& name, const BT::NodeConfiguration& config)
+    : BT::AsyncActionNode(name, config){}
+
+BT::NodeStatus Trajetory::tick() {
+    if (mgi_reference_->execute(trajetory_)) return BT::NodeStatus::SUCCESS;
+    return BT::NodeStatus::FAILURE;
+};
+
+void Trajetory::init(moveit::planning_interface::MoveGroupInterface* mgi_reference, moveit_msgs::RobotTrajectory& trajetory){
+    mgi_reference_ = mgi_reference;
+    trajetory_ = trajetory;
+}
\ No newline at end of file
diff --git a/src/impl/moveit_mediator.cpp b/src/impl/moveit_mediator.cpp
index 247672ef9e81d503c254aa80f91d9e754a3cb994..594f55c93e6f0f4909902c6586bb72586d8781aa 100644
--- a/src/impl/moveit_mediator.cpp
+++ b/src/impl/moveit_mediator.cpp
@@ -5,6 +5,20 @@
 #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_;
 
@@ -541,6 +555,9 @@ void Moveit_mediator::setup_task(){
 	}
 	*/
 
+	
+
+
 
 	std::regex item("box_([0-9]+)");
     std::smatch match;
@@ -585,7 +602,7 @@ void Moveit_mediator::setup_task(){
 							if (rt){
 								Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r);
 								// mr->mgi()->execute(*rt);
-								th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt, *ps));
+								//th.push_back(std::thread(&Moveit_mediator::parallel_exec, this, std::ref(*mr), *rt));
 							
 								// First find ID from panda to start with
 								std::regex rx_panda("panda_arm([0-9]+)");
@@ -737,16 +754,30 @@ void Moveit_mediator::task_planner(){
 	std::regex item("box_([0-9]+)");
     std::smatch match;
 
-	// circular buffer is job-q
+	// ----------------------------------
+	// 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::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal> tasks_per_robot;
-				// get rob by name
+				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);
 
@@ -757,6 +788,7 @@ void Moveit_mediator::task_planner(){
 						moveit_task_constructor_msgs::ExecuteTaskSolutionGoal e;
 						mgt.solutions().front()->fillMessage(e.solution, &mgt.introspection());
 						tasks_per_robot.push(e);
+						bt_list.push_back(e);
 
 						moveit_msgs::CollisionObject temp_co = psi_->getObjects().at(s_co.first);
 						temp_co.operation = temp_co.MOVE;
@@ -770,13 +802,120 @@ void Moveit_mediator::task_planner(){
 						psi_->applyCollisionObject(temp_co);
 					}
 				}
-				std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>> pair(temp.second.jobs_.front().getOrigin(), tasks_per_robot);
-				tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), pair));
+
+				/*
+				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}));
+				}
+
+				tasks_.insert(std::pair<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>(mr->name(), {temp.second.jobs_.front().getOrigin(), tasks_per_robot}));
 				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(this, 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();
+
+		for(int i = 0; i < executions_.size(); i++){
+			if(executions_[i]->joinable()) executions_[i]->join();
+		}
+		// IMPORTANT: you must always add some sleep if you call tickRoot()
+		// in a loop, to avoid using 100% of your CPU (busy loop).
+		// The method Tree::sleep() is recommended, because it can be
+		// interrupted by an internal event inside the tree.
+	}
+
+
 	// clean up
 	for (int i = 0; i < cd.size(); i ++){
 		std::stringstream ss;
@@ -825,6 +964,12 @@ void Moveit_mediator::merge_acm(moveit_msgs::PlanningScene& ps_m){
 
 }
 
+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;
@@ -867,10 +1012,16 @@ void Moveit_mediator::merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::Pla
 	}
 	
 }
-	
-void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt, moveit_msgs::PlanningScene ps){
+
+/*
+void Moveit_mediator::parallel_exec(Moveit_robot& mr, moveit_msgs::RobotTrajectory rt){
 	mr.mgi()->execute(rt);
 }
+*/
+void Moveit_mediator::parallel_exec(){
+	//ROS_INFO("UwU");
+	// mr.mgi()->execute(rt);
+}
 
 
 
@@ -1143,6 +1294,14 @@ moveit::task_constructor::Task Moveit_mediator::create_Task(Moveit_robot* mr, mo
 			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));
 	}
@@ -1270,6 +1429,7 @@ Moveit_mediator::Moveit_mediator(std::vector<std::vector<tf2::Transform>> object
 	, job_reader_(std::make_unique<Job_reader>(nh_))
     , cuboid_reader_(std::make_unique<Cuboid_reader>(nh)) {
 
+		executions_.resize(2);
 
         if (planning_scene_monitor_->getPlanningScene()){
             planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,