From 3f2ca8e66d9dfed6423a4f3c076ff81a7147fdb5 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 30 Aug 2023 17:12:56 +0200
Subject: [PATCH] Added some screen output to help debugging (to be removed).

---
 src/iri_bt_basic_nodes.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index 6151e2b..0e0859e 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
+}
-- 
GitLab