diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h
index 10bd101e30d2d9d76755f4c47b85d2c9e6ef13c0..5701dc1212062638aac190ebc311ebb4feb81b49 100644
--- a/include/iri_bt_basic_nodes.h
+++ b/include/iri_bt_basic_nodes.h
@@ -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
diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index ecf9ca11569d301a8a6cb44046f295422a1eb692..6151e2b7670133de9ee86924ce5a0787913bd622 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -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
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
new file mode 100644
index 0000000000000000000000000000000000000000..35e3ec719e87f19a363fc0ee85e94fcf76ee97ba
--- /dev/null
+++ b/src/xml/bt_definitions.xml
@@ -0,0 +1,41 @@
+<?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