Added leafnode name to prints

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);

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>