diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp
index bc9d306..c0c9073 100644
--- a/src/btree_nodes/src/Tree.cpp
+++ b/src/btree_nodes/src/Tree.cpp
@@ -164,7 +164,7 @@ int main(int argc, char **argv) {
     }).detach();
 
     std::cout << "Looping." << std::endl;
-
+    
     while (rclcpp::ok()) {
         blackboardMutex.lock();
         actorTree.tickRoot();
diff --git a/src/btree_nodes/src/nodes/ActorMovement.cpp b/src/btree_nodes/src/nodes/ActorMovement.cpp
index d84873b..c967ba7 100644
--- a/src/btree_nodes/src/nodes/ActorMovement.cpp
+++ b/src/btree_nodes/src/nodes/ActorMovement.cpp
@@ -3,6 +3,7 @@
 //
 
 #include "ActorMovement.h"
+#include <action_msgs/msg/detail/goal_status__struct.hpp>
 #include <behaviortree_cpp_v3/basic_types.h>
 
 
@@ -54,10 +55,16 @@ BT::NodeStatus ActorMovement::onStart() {
         mutex.unlock();
     };
     send_goal_options.goal_response_callback = [&](ClientGoalHandle<Movement>::SharedPtr parameter){
+        mutex.lock();
         if(!parameter){
             std::cout << "ActorMovement rejected by server." << std::endl;
             result = BT::NodeStatus::FAILURE;
+        }else{
+            if (parameter->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELED || parameter->get_status() == action_msgs::msg::GoalStatus::STATUS_CANCELING ){
+                result = BT::NodeStatus::FAILURE;
+            }
         }
+        mutex.unlock();
     };
 
     client->async_send_goal(goal,send_goal_options);
diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp
index e55e6c2..36ebe1b 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.cpp
+++ b/src/btree_nodes/src/nodes/MoveConnection.cpp
@@ -10,6 +10,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
                                     const std::function<void(bool)> &callback) {
     std::cout<<"planAndExecute."<<std::endl;
     lock.lock();
+    state = RUNNING;
     std::cout<<"got lock."<<std::endl;
     lastTarget = pose;
     lastCallback = callback;
@@ -19,27 +20,21 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
 
     std::cout<<"Starting planning"<<std::endl;
 
-    std::thread([this,callback,pose](){
-        
-        moveGroup.setPoseTarget(*pose);
+    std::thread([&](){
+        moveGroup.setPoseTarget(*lastTarget);
         moveGroup.setGoalJointTolerance(0.01);
         moveGroup.setGoalOrientationTolerance(0.01);
 
         std::cout<<"Parameters set."<<std::endl;
-
-        moveit::planning_interface::MoveGroupInterface::Plan plan;
-        if(moveGroup.plan(plan)==moveit::core::MoveItErrorCode::SUCCESS){
-            std::cout<<"Planned a path."<<std::endl;
-        }else{
-            std::cout<<"Error during planning."<<std::endl;
+        
+        auto finished = moveGroup.move() == moveit::core::MoveItErrorCode::SUCCESS;
+        
+        if(state!=SPEED_CHANGE){
+            lastCallback( finished );
             lastTarget = nullptr;
-            lock.unlock();
-            callback(false);
-            return;
         }
-        callback( moveGroup.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS );
-
-        lastTarget = nullptr;
+        
+        state = IDLE;
         lock.unlock();
     }).detach();
 }
@@ -51,6 +46,7 @@ void MoveConnection::setSpeedMultiplier(const double speed){
         auto target = lastTarget;
         auto callback = lastCallback;
         if (target != nullptr) {
+            state = SPEED_CHANGE;
             stop();
             std::cout<<"Restarting"<<std::endl;
             planAndExecute(target, callback);
@@ -60,11 +56,9 @@ void MoveConnection::setSpeedMultiplier(const double speed){
 
 void MoveConnection::stop() {
     std::cout<<"Stopping MoveConnection"<<std::endl;
-    cancelled = true;
-    if(rclcpp::ok()){
+    while (rclcpp::ok() && state!=IDLE){
         moveGroup.stop();
-        while (lastTarget != nullptr) {}
+        usleep(1000);
     }
-    cancelled = false;
     std::cout<<"Stopped MoveConnection"<<std::endl;
 }
diff --git a/src/btree_nodes/src/nodes/MoveConnection.h b/src/btree_nodes/src/nodes/MoveConnection.h
index f10dce1..a75e695 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.h
+++ b/src/btree_nodes/src/nodes/MoveConnection.h
@@ -12,10 +12,10 @@
 
 class MoveConnection {
 private:
+    enum {IDLE,RUNNING,SPEED_CHANGE} state;
     moveit::planning_interface::MoveGroupInterface moveGroup;
     std::shared_ptr<geometry_msgs::msg::Pose> lastTarget = nullptr;
-    std::function<void(bool)> lastCallback = [](bool finished) {};
-    bool cancelled=false;
+    std::function<void(bool)> lastCallback = [](__attribute__((unused))bool finished) {};
     double currentSpeed = 1;
     std::mutex lock;
 public:
diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp
index 7c22fd0..4dc85eb 100644
--- a/src/btree_nodes/src/nodes/RobotMove.cpp
+++ b/src/btree_nodes/src/nodes/RobotMove.cpp
@@ -11,6 +11,7 @@ BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &c
 }
 
 BT::NodeStatus BT::RobotMove::onStart() {
+    std::cout << "Starting RobotMove" << std::endl;
     std::shared_ptr<geometry_msgs::msg::Pose> pose;
     if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){
         std::cout << "no target available." << std::endl;
@@ -38,8 +39,9 @@ BT::NodeStatus BT::RobotMove::onRunning() {
 }
 
 void BT::RobotMove::onHalted() {
+    std::cout << "Halting RobotMove" << std::endl;
     connection->get()->stop();
-    asyncResult=NodeStatus::IDLE;
+    asyncResult=NodeStatus::FAILURE;
 }
 
 BT::PortsList BT::RobotMove::providedPorts() {
diff --git a/src/ros_actor_action_server/src/Server.cpp b/src/ros_actor_action_server/src/Server.cpp
index 2555f24..ece8c8a 100644
--- a/src/ros_actor_action_server/src/Server.cpp
+++ b/src/ros_actor_action_server/src/Server.cpp
@@ -80,7 +80,7 @@ int main(int argc, char **argv) {
     FeedbackMessage currentFeedback;
     ActionMessage currentAction;
 
-    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction] {
+    std::thread([feedbackQueue, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback] {
         while (true) {
             FeedbackMessage newFeedback;
             if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){