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