From 562e32b5aa0a58adfdda00781078591479ec3a47 Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Wed, 21 Feb 2024 13:00:23 +0100
Subject: [PATCH] Added leafnode name to prints

---
 src/iri_bt_basic_nodes.cpp | 35 +++++++++++++++++------------------
 src/xml/bt_definitions.xml |  8 ++++----
 2 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index 58780d4..a13d4b0 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -41,7 +41,7 @@ BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
 
   if(!variable)
   {
-    ROS_ERROR("IriBTBasicNodes::is_variable_true-> Incorrect or missing input. It needs the following input ports: variable (bool)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::is_variable_true (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: variable (bool)");
     return BT::NodeStatus::FAILURE;
   }
   bool variable_value=variable.value();
@@ -58,7 +58,7 @@ BT::NodeStatus IriBTBasicNodes::is_variable_false(BT::TreeNode& self)
 
   if(!variable)
   {
-    ROS_ERROR("IriBTBasicNodes::is_variable_false-> Incorrect or missing input. It needs the following input ports: variable (bool)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::is_variable_false (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: variable (bool)");
     return BT::NodeStatus::FAILURE;
   }
   bool variable_value=variable.value();
@@ -80,7 +80,7 @@ BT::NodeStatus IriBTBasicNodes::transform_pose(BT::TreeNode& self)
 
   if((!pose_in) || (!target_frame))
   {
-    ROS_ERROR("IriBTBasicNodes::transform_pose-> Incorrect or missing input. It needs the following input ports: pose_in (geometry_msgs::PoseStamped) target_frame (std::string) pose_out (geometry_msgs::PoseStamped)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::transform_pose (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose_in (geometry_msgs::PoseStamped) target_frame (std::string) pose_out (geometry_msgs::PoseStamped)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -122,7 +122,7 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
 
   if((!pose1) || (!pose2))
   {
-    ROS_ERROR("IriBTBasicNodes::compute_distance-> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -131,12 +131,11 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
 
   if (pose1_value.header.frame_id != pose1_value.header.frame_id)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance-> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
     return BT::NodeStatus::FAILURE;
   }
 
   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 << "distance: " << distance << std::endl;
   self.setOutput("distance",distance);
   return BT::NodeStatus::SUCCESS;
 }
@@ -152,7 +151,7 @@ BT::NodeStatus IriBTBasicNodes::compute_y_distance(BT::TreeNode& self)
 
   if((!pose1) || (!pose2))
   {
-    ROS_ERROR("IriBTBasicNodes::compute_y_distance-> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: pose1 (geometry_msgs::PoseStamped) pose2 (geometry_msgs::PoseStamped) distance (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -161,7 +160,7 @@ BT::NodeStatus IriBTBasicNodes::compute_y_distance(BT::TreeNode& self)
 
   if (pose1_value.header.frame_id != pose1_value.header.frame_id)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance-> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
+    ROS_ERROR_STREAM("IriBTBasicNodes::compute_y_distance (" << self.name() << ") -> Points with different frame ids: pose1=" << pose1_value.header.frame_id << "; pose2=" << pose1_value.header.frame_id);
     return BT::NodeStatus::FAILURE;
   }
 
@@ -179,7 +178,7 @@ BT::NodeStatus IriBTBasicNodes::compare_bigger(BT::TreeNode& self)
 
   if((!value1) || (!value2))
   {
-    ROS_ERROR("IriBTBasicNodes::compare_bigger-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -200,7 +199,7 @@ BT::NodeStatus IriBTBasicNodes::compare_smaller(BT::TreeNode& self)
 
   if((!value1) || (!value2))
   {
-    ROS_ERROR("IriBTBasicNodes::compare_smaller-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+    ROS_ERROR_STREAM("IriBTBasicNodes::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -227,7 +226,7 @@ BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
 
   if (!msg_p || !type_p)
   {
-    ROS_ERROR_STREAM("common_helpers::print_msg-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("common_helpers::print_msg (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " msg (sdt::string) and type (std::string)");
     return BT::NodeStatus::FAILURE;
   }
@@ -255,7 +254,7 @@ BT::NodeStatus IriBTBasicNodes::start_timeout(BT::TreeNode& _self)
 
   if (!name_p || !duration_p)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::start_timeout-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("IriBTBasicNodes::start_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " tout_name (sdt::string) and duration (double)");
     return BT::NodeStatus::FAILURE;
   }
@@ -282,7 +281,7 @@ BT::NodeStatus IriBTBasicNodes::stop_timeout(BT::TreeNode& _self)
 
   if (!name_p)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " tout_name (sdt::string)");
     return BT::NodeStatus::FAILURE;
   }
@@ -291,7 +290,7 @@ BT::NodeStatus IriBTBasicNodes::stop_timeout(BT::TreeNode& _self)
   it = this->timeouts.find(name);
   if (it == this->timeouts.end())
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout-> No timeout found with name " << name);
+    ROS_ERROR_STREAM("IriBTBasicNodes::stop_timeout (" << _self.name() << ") -> No timeout found with name " << name);
     return BT::NodeStatus::FAILURE;
   }
   it->second.stop();
@@ -309,7 +308,7 @@ BT::NodeStatus IriBTBasicNodes::get_timed_out(BT::TreeNode& _self)
   _self.setOutput("timed_out", timed_out);
   if (!name_p)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " tout_name (sdt::string)");
     return BT::NodeStatus::FAILURE;
   }
@@ -318,7 +317,7 @@ BT::NodeStatus IriBTBasicNodes::get_timed_out(BT::TreeNode& _self)
   it = this->timeouts.find(name);
   if (it == this->timeouts.end())
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out-> No timeout found with name " << name);
+    ROS_ERROR_STREAM("IriBTBasicNodes::get_timed_out (" << _self.name() << ") -> No timeout found with name " << name);
     return BT::NodeStatus::FAILURE;
   }
   timed_out = it->second.timed_out();
@@ -334,7 +333,7 @@ BT::NodeStatus IriBTBasicNodes::erase_timeout(BT::TreeNode& _self)
 
   if (!name_p)
   {
-    ROS_ERROR_STREAM("IriBTBasicNodes::erase_timeout-> Incorrect or missing input. It needs the following input ports:"
+    ROS_ERROR_STREAM("IriBTBasicNodes::erase_timeout (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
                       << " tout_name (sdt::string)");
     return BT::NodeStatus::FAILURE;
   }
@@ -343,7 +342,7 @@ BT::NodeStatus IriBTBasicNodes::erase_timeout(BT::TreeNode& _self)
   it = this->timeouts.find(name);
   if (it == this->timeouts.end())
   {
-    ROS_WARN_STREAM("IriBTBasicNodes::erase_timeout-> No timeout found with name " << name);
+    ROS_WARN_STREAM("IriBTBasicNodes::erase_timeout (" << _self.name() << ") -> No timeout found with name " << name);
     return BT::NodeStatus::SUCCESS;
   }
   this->timeouts.erase(it);
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
index 8471bf2..9b5cb3e 100644
--- a/src/xml/bt_definitions.xml
+++ b/src/xml/bt_definitions.xml
@@ -8,12 +8,12 @@
     <TreeNodesModel>
         <!-- basic_nodes -->
         <Action ID="RUNNING"/>
-        <Action ID="is_variable_true">
+        <Condition ID="is_variable_true">
             <input_port name="variable"> (bool): Bool variable to check.</input_port>
-        </Action>
-        <Action ID="is_variable_false">
+        </Condition>
+        <Condition ID="is_variable_false">
             <input_port name="variable"> (bool): Bool variable to check.</input_port>
-        </Action>
+        </Condition>
         <Action ID="transform_pose">
             <input_port name="pose_in"> (geometry_msgs::PoseStamped): Pose to be transformed.</input_port>
             <input_port name="target_frame"> (std::string): Frame to transform the pose.</input_port>
-- 
GitLab