Skip to content
Snippets Groups Projects
Commit 562e32b5 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added leafnode name to prints

parent d0a4f01a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/iri_bt_basic_nodes.cpp
+ 17
− 18
View file @ 562e32b5
......@@ -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
+ 4
− 4
View file @ 562e32b5
......@@ -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>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment