Added some screen output to help debugging (to be removed).

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
+}