diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index 6151e2b7670133de9ee86924ce5a0787913bd622..0e0859e7a64295c8d598271a70de2f62dab5c375 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -128,7 +128,9 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
   }
 
   distance=sqrt(pow(pose1_value.pose.position.x-pose2_value.pose.position.x,2.0)+pow(pose1_value.pose.position.y-pose2_value.pose.position.y,2.0)+pow(pose1_value.pose.position.z-pose2_value.pose.position.z,2.0));
-
+  std::cout << "pose1: x: " << pose1_value.pose.position.x << ", y: " << pose1_value.pose.position.y << ", z: " << pose1_value.pose.position.z << std::endl;
+  std::cout << "pose1: x: " << pose2_value.pose.position.x << ", y: " << pose2_value.pose.position.y << ", z: " << pose2_value.pose.position.z << std::endl;
+  std::cout << "distance: " << distance << std::endl;
   self.setOutput("distance",distance);
   return BT::NodeStatus::SUCCESS;
 }
@@ -205,4 +207,4 @@ BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
   else
     ROS_INFO_STREAM(msg);
   return BT::NodeStatus::SUCCESS;
-}
\ No newline at end of file
+}