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

Updated documentation. Added bt_definitions. Added print_msg function

parent 7a30823b
No related branches found
No related tags found
1 merge request!1Revert "Added two actions: one to generate a success state and the other one...
......@@ -37,12 +37,119 @@ class IriBTBasicNodes
*
*/
BT::NodeStatus RUNNING(void);
/**
* \brief Check that a bool variable is true or not.
*
* It has the following input ports:
*
* variable (bool): Bool variable to check.
*
* \return BT:NodeStatus::SUCCESS if variable is true. Otherwise returns
* BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus is_variable_true(BT::TreeNode& self);
/**
* \brief Check that a bool variable is false or not.
*
* It has the following input ports:
*
* variable (bool): Bool variable to check.
*
* \return BT:NodeStatus::SUCCESS if variable is false. Otherwise returns
* BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus is_variable_false(BT::TreeNode& self);
/**
* \brief Function to transform a pose to a target frame.
*
* It has the following input ports:
*
* pose_in (geometry_msgs::PoseStamped): Pose to be transformed.
*
* target_frame (std::string): Frame to transform the pose.
*
* It also has the following output ports:
*
* pose_out (geometry_msgs::PoseStamped): Transformed pose.
*
* \return BT:NodeStatus::SUCCESS if pose can be transformed.
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus transform_pose(BT::TreeNode& self);
/**
* \brief Function to Compute the distance between two points.
* They must be in thew same frame.
*
* It has the following input ports:
*
* pose1 (geometry_msgs::PoseStamped): Pose 1.
*
* pose2 (geometry_msgs::PoseStamped): Pose 2.
*
* It also has the following output ports:
*
* distance (double): Distance.
*
* \return BT:NodeStatus::SUCCESS if distance can be computed.
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus compute_distance(BT::TreeNode& self);
/**
* \brief Function to compare if a double is bigger than other.
*
* It has the following input ports:
*
* value1 (double): Value 1.
*
* value2 (double): Value 2.
*
* \return BT:NodeStatus::SUCCESS if value1 is bigger that value2.
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus compare_bigger(BT::TreeNode& self);
BT::NodeStatus compare_smaller(BT::TreeNode& self);
};
/**
* \brief Function to compare if a double is smaller than other.
*
* It has the following input ports:
*
* value1 (double): Value 1.
*
* value2 (double): Value 2.
*
* \return BT:NodeStatus::SUCCESS if value1 is smaller that value2.
* Otherwise returns BT:NodeStatus::FAILURE. If inputs are missing or incorrect it also
* returns BT:NodeStatus::FAILURE.
*/
BT::NodeStatus compare_smaller(BT::TreeNode& self);
/**
* \brief Function to write in terminal.
*
* It has the following input ports:
*
* msg (std::string): Msg to print.
*
* type (std::string): It must be "info", "warn", "error" or "debug".
*
* \param _self node with the required ports:
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE).
* If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE.
*
*/
BT::NodeStatus print_msg(BT::TreeNode& _self);
};
#endif
......@@ -12,6 +12,7 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
BT::PortsList transform_pose_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose_in"),BT::InputPort<std::string>("target_frame"),BT::OutputPort<geometry_msgs::PoseStamped>("pose_out")};
BT::PortsList compute_distance_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose1"),BT::InputPort<geometry_msgs::PoseStamped>("pose2"),BT::OutputPort<double>("distance")};
BT::PortsList compare_ports = {BT::InputPort<double>("value1"),BT::InputPort<double>("value2")};
BT::PortsList print_msg_ports = {BT::InputPort<std::string>("msg"), BT::InputPort<std::string>("type")};
//Registration of conditions
factory.registerSimpleCondition("is_variable_true", std::bind(&IriBTBasicNodes::is_variable_true, this, std::placeholders::_1),check_variable_ports);
......@@ -22,6 +23,7 @@ void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("compute_distance", std::bind(&IriBTBasicNodes::compute_distance, this, std::placeholders::_1),compute_distance_ports);
factory.registerSimpleAction("compare_bigger", std::bind(&IriBTBasicNodes::compare_bigger, this, std::placeholders::_1),compare_ports);
factory.registerSimpleAction("compare_smaller", std::bind(&IriBTBasicNodes::compare_smaller, this, std::placeholders::_1),compare_ports);
factory.registerSimpleAction("print_msg", std::bind(&IriBTBasicNodes::print_msg, this, std::placeholders::_1), print_msg_ports);
}
BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
......@@ -64,15 +66,20 @@ BT::NodeStatus IriBTBasicNodes::transform_pose(BT::TreeNode& self)
BT::Optional<geometry_msgs::PoseStamped> pose_in = self.getInput<geometry_msgs::PoseStamped>("pose_in");
BT::Optional<std::string> target_frame = self.getInput<std::string>("target_frame");
geometry_msgs::PoseStamped new_pose;
//Set a default value
self.setOutput("pose_out", new_pose);
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)");
return BT::NodeStatus::FAILURE;
}
geometry_msgs::PoseStamped pose_in_value=pose_in.value(),new_pose;
geometry_msgs::PoseStamped pose_in_value=pose_in.value();
std::string target_frame_value=target_frame.value();
if(target_frame_value!=pose_in_value.header.frame_id)
{
try{
......@@ -102,6 +109,9 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
BT::Optional<geometry_msgs::PoseStamped> pose1 = self.getInput<geometry_msgs::PoseStamped>("pose1");
BT::Optional<geometry_msgs::PoseStamped> pose2 = self.getInput<geometry_msgs::PoseStamped>("pose2");
double distance;
self.setOutput("distance",distance);
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)");
......@@ -110,7 +120,12 @@ BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
geometry_msgs::PoseStamped pose1_value=pose1.value();
geometry_msgs::PoseStamped pose2_value=pose2.value();
double distance;
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);
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));
......@@ -165,3 +180,29 @@ BT::NodeStatus IriBTBasicNodes::RUNNING()
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus IriBTBasicNodes::print_msg(BT::TreeNode& _self)
{
BT::Optional<std::string> msg_p = _self.getInput<std::string>("msg");
BT::Optional<std::string> type_p = _self.getInput<std::string>("type");
std::string msg;
std::string type;
if (!msg_p || !type_p)
{
ROS_ERROR_STREAM("common_helpers::print_msg-> Incorrect or missing input. It needs the following input ports:"
<< " msg (sdt::string) and type (std::string)");
return BT::NodeStatus::FAILURE;
}
msg = msg_p.value();
type = type_p.value();
if (type == "warn")
ROS_WARN_STREAM(msg);
else if (type == "error")
ROS_ERROR_STREAM(msg);
else if (type == "debug")
ROS_DEBUG_STREAM(msg);
else
ROS_INFO_STREAM(msg);
return BT::NodeStatus::SUCCESS;
}
\ No newline at end of file
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Action ID="RUNNING"/>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<!-- basic_nodes -->
<Action ID="RUNNING"/>
<Action ID="is_variable_true">
<input_port name="variable"> (bool): Bool variable to check.</input_port>
</Action>
<Action ID="is_variable_false">
<input_port name="variable"> (bool): Bool variable to check.</input_port>
</Action>
<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>
<output_port name="pose_out"> (geometry_msgs::PoseStamped): Transformed pose.</output_port>
</Action>
<Action ID="compute_distance">
<input_port name="pose1"> (geometry_msgs::PoseStamped): Pose 1.</input_port>
<input_port name="pose2"> (geometry_msgs::PoseStamped): Pose 2.</input_port>
<output_port name="distance"> (double): Distance.</output_port>
</Action>
<Action ID="compare_bigger">
<input_port name="value1"> (double): Value 1.</input_port>
<input_port name="value2"> (double): Value 2.</input_port>
</Action>
<Action ID="compare_smaller">
<input_port name="value1"> (double): Value 1.</input_port>
<input_port name="value2"> (double): Value 2.</input_port>
</Action>
<Action ID="print_msg">
<input_port name="msg"> (std::string): Msg to print.</input_port>
<input_port name="type"> (std::string): It must be info, warn, error or debug.</input_port>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment