diff --git a/src/btree_nodes/src/nodes/ActorAnimation.cpp b/src/btree_nodes/src/nodes/ActorAnimation.cpp
index 847c2fe..9484d18 100644
--- a/src/btree_nodes/src/nodes/ActorAnimation.cpp
+++ b/src/btree_nodes/src/nodes/ActorAnimation.cpp
@@ -44,12 +44,16 @@ BT::NodeStatus ActorAnimation::onStart() {
 
     client->async_send_goal(goal,send_goal_options);
 
+    result = BT::NodeStatus::RUNNING;
     return BT::NodeStatus::RUNNING;
 }
 
 BT::NodeStatus ActorAnimation::onRunning() {
     mutex.lock();
     auto status = result;
+    if(result != BT::NodeStatus::RUNNING){
+        result = BT::NodeStatus::IDLE;
+    }
     mutex.unlock();
     return status;
 }
diff --git a/src/btree_nodes/src/nodes/OffsetPose.cpp b/src/btree_nodes/src/nodes/OffsetPose.cpp
index bca28bb..236ffc8 100644
--- a/src/btree_nodes/src/nodes/OffsetPose.cpp
+++ b/src/btree_nodes/src/nodes/OffsetPose.cpp
@@ -3,14 +3,16 @@
 //
 
 #include "OffsetPose.h"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <memory>
 
 
 BT::PortsList BT::OffsetPose::providedPorts() {
     return {
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Position2D"),
+            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Pose"),
             InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
             InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
-            InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
+            OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
     };
 }
 
@@ -18,28 +20,30 @@ BT::OffsetPose::OffsetPose(const std::string &name,
         const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}
 
 BT::NodeStatus BT::OffsetPose::tick() {
-
+    
     std::shared_ptr<geometry_msgs::msg::Pose> pose;
     if (!getInput("input", pose)) {
         return NodeStatus::FAILURE;
     }
+    
+    auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);
 
     std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
     if(getInput("orientation", quaternion)) {
-        pose->orientation.w = quaternion->w;
-        pose->orientation.x = quaternion->x;
-        pose->orientation.y = quaternion->y;
-        pose->orientation.z = quaternion->z;
+        resultPose->orientation.w = quaternion->w;
+        resultPose->orientation.x = quaternion->x;
+        resultPose->orientation.y = quaternion->y;
+        resultPose->orientation.z = quaternion->z;
     }
 
     std::shared_ptr<geometry_msgs::msg::Point> offset;
     if(getInput("offset", offset)) {
-        pose->position.x += offset->x;
-        pose->position.y += offset->y;
-        pose->position.z += offset->z;
+        resultPose->position.x += offset->x;
+        resultPose->position.y += offset->y;
+        resultPose->position.z += offset->z;
     }
 
-    setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose);
+    setOutput("output", resultPose);
 
     return BT::NodeStatus::SUCCESS;
 }
diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp
index 4dc85eb..0614ca7 100644
--- a/src/btree_nodes/src/nodes/RobotMove.cpp
+++ b/src/btree_nodes/src/nodes/RobotMove.cpp
@@ -13,7 +13,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)){
+    if(!getInput("target",pose)){
         std::cout << "no target available." << std::endl;
         return NodeStatus::FAILURE;
     }
@@ -22,8 +22,10 @@ BT::NodeStatus BT::RobotMove::onStart() {
     asyncResult = NodeStatus::RUNNING;
     connection->get()->planAndExecute(pose,[this](bool finished){
         if(finished){
+            printf("Finished move.");
             asyncResult = NodeStatus::SUCCESS;
         } else {
+            printf("Failed move.");
             asyncResult = NodeStatus::FAILURE;
         }
     });