diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt
index 46bb692..a268ef5 100644
--- a/src/btree_nodes/CMakeLists.txt
+++ b/src/btree_nodes/CMakeLists.txt
@@ -22,6 +22,7 @@ set(DEPENDENCIES
         tf2_geometry_msgs
         moveit_ros_planning
         pluginlib
+        ros_actor_action_server_msgs
         )
 
 foreach(dep IN LISTS DEPENDENCIES)
@@ -48,6 +49,7 @@ set(CPP_FILES
         src/nodes/InAreaTest.cpp
         src/nodes/InterruptableSequence.cpp
         src/nodes/SetRobotVelocity.cpp
+        src/nodes/ActorMovement.cpp
         )
 
 add_library(tree_plugins_base src/Factory.cpp)
diff --git a/src/btree_nodes/package.xml b/src/btree_nodes/package.xml
index a703b55..5424bb2 100644
--- a/src/btree_nodes/package.xml
+++ b/src/btree_nodes/package.xml
@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>ros_actor_action_server_msgs</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp
index e615bf9..83dc29a 100644
--- a/src/btree_nodes/src/Tree.cpp
+++ b/src/btree_nodes/src/Tree.cpp
@@ -13,6 +13,7 @@
 #include "nodes/InAreaTest.h"
 #include "nodes/InterruptableSequence.h"
 #include "nodes/SetRobotVelocity.h"
+#include "nodes/ActorMovement.h"
 #include <chrono>
 #include <thread>
 
@@ -23,7 +24,8 @@ int main(int argc, char **argv) {
     rclcpp::NodeOptions node_options;
     node_options.automatically_declare_parameters_from_overrides(true);
 
-    auto mainNode = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);
+    auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options);
+    auto moveNode = rclcpp::Node::make_shared("tree_moveit_node", node_options);
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(mainNode);
@@ -33,46 +35,46 @@ int main(int argc, char **argv) {
 
     const std::shared_ptr<Area> safeArea = std::make_shared<Area>();
     safeArea->triangles = {std::vector<Position2D>({
-            {-1, 3.5},
-            {-1, 7},
-            {3,  3.5},
-            {7,  7},
-            {3,  -1},
-            {7,  -1}
+            {   1,     3.5 },
+            {   1,     7   },
+            {   3,     3.5 },
+            {   7,     7   },
+            {   3,    -1   },
+            {   7,    -1   }
     })};
 
     const std::shared_ptr<Area> warningArea = std::make_shared<Area>();
     warningArea->triangles = {std::vector<Position2D>({
-            {-1, 2.5},
-            {-1, 3.5},
-            {2,  2.5},
-            {3,  3.5},
-            {2,  -1},
-            {3,  -1}
+            {  -1,     2.5 },
+            {  -1,     3.5 },
+            {   2,     2.5 },
+            {   3,     3.5 },
+            {   2,    -1   },
+            {   3,    -1   }
     })};
 
     const std::shared_ptr<Area> unsafeArea = std::make_shared<Area>();
     unsafeArea->triangles = std::vector<Position2D>({
-            {-1, 1.5},
-            {-1, 2.5},
-            {1,  1.5},
-            {2,  2.5},
-            {1,  -1},
-            {2,  -1}
+            {  -1,     1.5 },
+            {  -1,     2.5 },
+            {   1,     1.5 },
+            {   2,     2.5 },
+            {   1,    -1   },
+            {   2,    -1   }
     });
 
     const std::shared_ptr<Area> negativeYTable = std::make_shared<Area>();
     negativeYTable->triangles = std::vector<Position2D>({
-            {0.3,-0.25},
-            {-0.3,-0.25},
-            {0,-0.4}
+            {   0.3,   -0.25    },
+            {  -0.3,   -0.25    },
+            {   0,     -0.4     }
     });
 
     const std::shared_ptr<Area> positiveYTable = std::make_shared<Area>();
     positiveYTable->triangles = std::vector<Position2D>({
-            {0.3,0.25},
-            {-0.3,0.25},
-            {0,0.4}
+            {   0.3,    0.25    },
+            {  -0.3,    0.25    },
+            {     0,    0.4     }
     });
 
     std::cout << "Registering nodes." << std::endl;
@@ -80,6 +82,7 @@ int main(int argc, char **argv) {
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
     factory.registerNodeType<AmICalled>("AmICalled");
 
+    factory.registerNodeType<ActorMovement>("ActorMovement");
     factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");
@@ -88,7 +91,7 @@ int main(int argc, char **argv) {
 
     factory.registerNodeType<InterruptableSequence>("InterruptableSequence");
 
-    auto connection = std::make_shared<MoveConnection>(mainNode, "iisy");
+    auto connection = std::make_shared<MoveConnection>(moveNode, "arm");
 
     NodeBuilder builderIisyMove = [&connection](const std::string &name, const NodeConfiguration &config) {
         return std::make_unique<RobotMove>(name, config, &connection);
@@ -119,11 +122,13 @@ int main(int argc, char **argv) {
 
     std::cout << "Creating tree." << std::endl;
 
-    Tree actorTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/actorTree.xml");
-    Tree robotTree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/robotTree.xml");
+    Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml");
+    Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml");
 
     auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()};
     auto init = std::make_shared<geometry_msgs::msg::Pose>();
+    init->position.x = 6.0;
+    init->position.y = 6.0;
 
     for (const auto &item : trees){
         item->set("safeArea",safeArea);
@@ -147,12 +152,12 @@ int main(int argc, char **argv) {
 
     std::cout << "Looping." << std::endl;
 
-    std::thread actor([&actorTree]() {
+    /*std::thread actor([&actorTree]() {
         while (rclcpp::ok()) {
             actorTree.tickRoot();
             std::this_thread::sleep_for(std::chrono::milliseconds(20));
         }
-    });
+    });*/
 
     while (rclcpp::ok()) {
         robotTree.tickRoot();
diff --git a/src/btree_nodes/src/nodes/ActorMovement.cpp b/src/btree_nodes/src/nodes/ActorMovement.cpp
new file mode 100644
index 0000000..89f459e
--- /dev/null
+++ b/src/btree_nodes/src/nodes/ActorMovement.cpp
@@ -0,0 +1,64 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#include "ActorMovement.h"
+
+BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
+        : StatefulActionNode(name, config) {}
+
+BT::PortsList BT::ActorMovement::providedPorts() {
+    return {
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
+    };
+}
+
+BT::NodeStatus BT::ActorMovement::onStart() {
+    std::cout << "started moving" << std::endl;
+
+    rclcpp::Node node("targetPublisherNode");
+    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
+
+    if (!res) {
+        std::cout << "[ no target available ]" << std::endl;
+        std::cout << res.error() << std::endl;
+        return NodeStatus::FAILURE;
+    }
+
+    publisher->publish(*target);
+    return NodeStatus::RUNNING;
+}
+
+BT::NodeStatus BT::ActorMovement::onRunning() {
+    std::shared_ptr<geometry_msgs::msg::Pose> current;
+
+    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
+    if (!res) {
+        std::cout << "[ no current position available ]" << std::endl;
+        std::cout << res.error() << std::endl;
+        return NodeStatus::FAILURE;
+    }
+
+    double dX = target->position.x - current->position.x;
+    double dY = target->position.y - current->position.y;
+
+    auto distance = sqrt(dX * dX + dY * dY);
+
+    if (distance < 0.3) {
+        return NodeStatus::SUCCESS;
+    } else {
+        return NodeStatus::RUNNING;
+    }
+}
+
+void BT::ActorMovement::onHalted() {
+    std::cout << "halted move" << std::endl;
+    rclcpp::Node node("targetPublisherNode");
+    auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
+    geometry_msgs::msg::Pose inf;
+    inf.position.x = HUGE_VAL;
+    inf.position.y = HUGE_VAL;
+    publisher->publish(inf);
+}
diff --git a/src/btree_nodes/src/nodes/ActorMovement.h b/src/btree_nodes/src/nodes/ActorMovement.h
new file mode 100644
index 0000000..bdbaa04
--- /dev/null
+++ b/src/btree_nodes/src/nodes/ActorMovement.h
@@ -0,0 +1,30 @@
+//
+// Created by bastian on 26.03.22.
+//
+
+#ifndef BUILD_ACTORMOVEMENT_H
+#define BUILD_ACTORMOVEMENT_H
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace BT {
+    class ActorMovement : public StatefulActionNode {
+    public:
+        ActorMovement(const std::string &name, const NodeConfiguration &config);
+
+        static PortsList providedPorts();
+
+        std::shared_ptr<geometry_msgs::msg::Pose> target;
+
+        NodeStatus onStart() override;
+
+        NodeStatus onRunning() override;
+
+        void onHalted() override;
+    };
+}
+
+
+#endif //BUILD_ACTORMOVEMENT_H
diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp
index ae87a70..8460628 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.cpp
+++ b/src/btree_nodes/src/nodes/MoveConnection.cpp
@@ -6,32 +6,39 @@
 
 void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Pose> &pose,
                                     const std::function<void(bool)> &callback) {
+    std::cout<<"planAndExecute."<<std::endl;
     lock.lock();
+    std::cout<<"got lock."<<std::endl;
     lastTarget = pose;
     lastCallback = callback;
 
     moveGroup.setMaxVelocityScalingFactor(currentSpeed);
     moveGroup.setMaxAccelerationScalingFactor(1);
 
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (moveGroup.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-    if(!success){
-        callback(false);
-        return;
-    }
+    std::cout<<"Starting planning"<<std::endl;
 
     std::thread([this,callback,pose](){
-        moveGroup.setStartStateToCurrentState();
+        
         moveGroup.setPoseTarget(*pose);
+        moveGroup.setGoalJointTolerance(0.01);
+        moveGroup.setGoalOrientationTolerance(0.01);
+
+        std::cout<<"Parameters set."<<std::endl;
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveGroup.plan(plan);
+        if(moveGroup.plan(plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS){
+            std::cout<<"Planned a path."<<std::endl;
+        }else{
+            std::cout<<"Error during planning."<<std::endl;
+            lock.unlock();
+            callback(false);
+        }
         if(cancelled){
             lastTarget = nullptr;
             lock.unlock();
             return;
         }
-        bool success = this->moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
+        bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
         if(!cancelled) {
             callback(success);
         }
diff --git a/src/iisy_config_2/config/moveit_controllers.yaml b/src/iisy_config_2/config/moveit_controllers.yaml
index 3ad47a3..e278a35 100644
--- a/src/iisy_config_2/config/moveit_controllers.yaml
+++ b/src/iisy_config_2/config/moveit_controllers.yaml
@@ -1,4 +1,6 @@
 # MoveIt uses this configuration for controller management
+trajectory_execution:
+  allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout
 
 moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
 
diff --git a/src/moveit_test/src/test.cpp b/src/moveit_test/src/test.cpp
index d9a6203..156d741 100644
--- a/src/moveit_test/src/test.cpp
+++ b/src/moveit_test/src/test.cpp
@@ -21,14 +21,18 @@ int main(int argc, char * argv[])
     // Set a target Pose
     auto const target_pose = []{
         geometry_msgs::msg::Pose msg;
-        msg.orientation.w = 1.0;
-        msg.position.x = 0.25;
-        msg.position.y = 0.25;
-        msg.position.z = 1.25;
+        msg.orientation.w = 0.0;
+        msg.orientation.x = 0.0;
+        msg.orientation.y = 1.0;
+        msg.orientation.z = 0.0;
+        msg.position.x = -0.043357;
+        msg.position.y = -0.281366;
+        msg.position.z = 1.1;
         return msg;
     }();
 
-    move_group_interface.setGoalOrientationTolerance(100000);
+    move_group_interface.setGoalOrientationTolerance(0.01);
+    move_group_interface.setGoalPositionTolerance(0.01);
     move_group_interface.setPoseTarget(target_pose);
 
     //std::map<std::string,double> states = {