From 6ea7fa9e8509227431e7687fc69fddb8369435ec Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 10 May 2023 07:54:59 +0200
Subject: [PATCH] Added several basic BT conditions and actions.
---
include/iri_bt_basic_nodes.h | 29 +++---
src/iri_bt_basic_nodes.cpp | 178 ++++++++++++++++++++++++-----------
2 files changed, 135 insertions(+), 72 deletions(-)
diff --git a/include/iri_bt_basic_nodes.h b/include/iri_bt_basic_nodes.h
index d26eaaa..10bd101 100644
--- a/include/iri_bt_basic_nodes.h
+++ b/include/iri_bt_basic_nodes.h
@@ -2,6 +2,8 @@
#define IRI_BT_BASIC_NODES_H
#include "iri_bt_factory.h"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
class IriBTBasicNodes
{
@@ -19,12 +21,10 @@ class IriBTBasicNodes
*/
void init(IriBehaviorTreeFactory &factory);
- void start_tree();
- void stop_tree();
private:
// variables to know whether start or stop have been demanded by the user
- bool tree_start;
- bool tree_stop;
+ tf2_ros::Buffer tf2_buffer;
+ tf2_ros::TransformListener tf2_listener;
protected:
/**
@@ -36,20 +36,13 @@ class IriBTBasicNodes
* \return a BT:NodeStatus::RUNNING always
*
*/
- BT::NodeStatus NOP(void);
- // functions to check whether start or restart have been demanded by the user
- BT::NodeStatus is_not_start_tree(void);
- BT::NodeStatus async_is_start_tree(void);
- BT::NodeStatus is_not_stop_tree(void);
- BT::NodeStatus async_is_stop_tree(void);
-
- // functions set start or restart variables
- BT::NodeStatus set_start_tree(void);
- BT::NodeStatus set_stop_tree(void);
-
- // functions reset start or restart variables
- BT::NodeStatus reset_start_tree(void);
- BT::NodeStatus reset_stop_tree(void);
+ BT::NodeStatus RUNNING(void);
+ BT::NodeStatus is_variable_true(BT::TreeNode& self);
+ BT::NodeStatus is_variable_false(BT::TreeNode& self);
+ BT::NodeStatus transform_pose(BT::TreeNode& self);
+ BT::NodeStatus compute_distance(BT::TreeNode& self);
+ BT::NodeStatus compare_bigger(BT::TreeNode& self);
+ BT::NodeStatus compare_smaller(BT::TreeNode& self);
};
#endif
diff --git a/src/iri_bt_basic_nodes.cpp b/src/iri_bt_basic_nodes.cpp
index e0efa60..ecf9ca1 100644
--- a/src/iri_bt_basic_nodes.cpp
+++ b/src/iri_bt_basic_nodes.cpp
@@ -1,97 +1,167 @@
#include "iri_bt_basic_nodes.h"
#include <ros/ros.h>
-IriBTBasicNodes::IriBTBasicNodes()
+IriBTBasicNodes::IriBTBasicNodes() :
+ tf2_listener(tf2_buffer)
{
- this->tree_start = false;
- this->tree_stop = false;
}
void IriBTBasicNodes::init(IriBehaviorTreeFactory &factory)
{
- factory.registerIriAsyncAction("NOP", std::bind(&IriBTBasicNodes::NOP, this));
-
- factory.registerSimpleCondition("is_not_start_tree", std::bind(&IriBTBasicNodes::is_not_start_tree, this));
- factory.registerIriAsyncAction("async_is_start_tree", std::bind(&IriBTBasicNodes::async_is_start_tree, this));
- factory.registerSimpleCondition("is_not_stop_tree", std::bind(&IriBTBasicNodes::is_not_stop_tree, this));
- factory.registerIriAsyncAction("async_is_stop_tree", std::bind(&IriBTBasicNodes::async_is_stop_tree, this));
-
- factory.registerSimpleAction("set_start_tree", std::bind(&IriBTBasicNodes::set_start_tree, this));
- factory.registerSimpleAction("set_stop_tree", std::bind(&IriBTBasicNodes::set_stop_tree, this));
- factory.registerSimpleAction("reset_start_tree", std::bind(&IriBTBasicNodes::reset_start_tree, this));
- factory.registerSimpleAction("reset_stop_tree", std::bind(&IriBTBasicNodes::reset_stop_tree, this));
+ BT::PortsList check_variable_ports = {BT::InputPort<bool>("variable")};
+ 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")};
+
+ //Registration of conditions
+ factory.registerSimpleCondition("is_variable_true", std::bind(&IriBTBasicNodes::is_variable_true, this, std::placeholders::_1),check_variable_ports);
+ factory.registerSimpleCondition("is_variable_false", std::bind(&IriBTBasicNodes::is_variable_false, this, std::placeholders::_1),check_variable_ports);
+
+ factory.registerIriAsyncAction("RUNNING", std::bind(&IriBTBasicNodes::RUNNING, this));
+ factory.registerSimpleAction("transform_pose", std::bind(&IriBTBasicNodes::transform_pose, this, std::placeholders::_1),transform_pose_ports);
+ 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);
}
-BT::NodeStatus IriBTBasicNodes::NOP()
+BT::NodeStatus IriBTBasicNodes::is_variable_true(BT::TreeNode& self)
{
- return BT::NodeStatus::RUNNING;
-}
+ ROS_DEBUG("IriBTBasicNodes::is_variable_true");
+ BT::Optional<bool> variable = self.getInput<bool>("variable");
-BT::NodeStatus IriBTBasicNodes::is_not_start_tree(void)
-{
- if(this->tree_start)
+ if(!variable)
+ {
+ ROS_ERROR("IriBTBasicNodes::is_variable_true-> Incorrect or missing input. It needs the following input ports: variable (bool)");
return BT::NodeStatus::FAILURE;
- else
+ }
+ bool variable_value=variable.value();
+ if(variable_value)
return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
}
-BT::NodeStatus IriBTBasicNodes::async_is_start_tree(void)
+BT::NodeStatus IriBTBasicNodes::is_variable_false(BT::TreeNode& self)
{
- if(this->tree_start)
+ ROS_DEBUG("IriBTBasicNodes::is_variable_false");
+ BT::Optional<bool> variable = self.getInput<bool>("variable");
+
+ if(!variable)
+ {
+ ROS_ERROR("IriBTBasicNodes::is_variable_false-> Incorrect or missing input. It needs the following input ports: variable (bool)");
+ return BT::NodeStatus::FAILURE;
+ }
+ bool variable_value=variable.value();
+ if(!variable_value)
return BT::NodeStatus::SUCCESS;
else
- return BT::NodeStatus::RUNNING;
+ return BT::NodeStatus::FAILURE;
}
-BT::NodeStatus IriBTBasicNodes::is_not_stop_tree(void)
+BT::NodeStatus IriBTBasicNodes::transform_pose(BT::TreeNode& self)
{
- if(this->tree_stop)
+ ROS_DEBUG("IriBTBasicNodes::transform_pose");
+ 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");
+
+ 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;
+ std::string target_frame_value=target_frame.value();
+
+ if(target_frame_value!=pose_in_value.header.frame_id)
+ {
+ try{
+ bool tf_exists = this->tf2_buffer.canTransform(target_frame_value, pose_in_value.header.frame_id, pose_in_value.header.stamp, ros::Duration(0.1));
+ if(tf_exists)
+ {
+ this->tf2_buffer.transform(pose_in_value, new_pose,target_frame_value);
+ self.setOutput("pose_out", new_pose);
+ return BT::NodeStatus::SUCCESS;
+ }
+ else
+ return BT::NodeStatus::FAILURE;
+ }catch(tf2::TransformException &ex){
+ return BT::NodeStatus::FAILURE;
+ }
+ }
else
+ {
+ self.setOutput("pose_out", pose_in_value);
return BT::NodeStatus::SUCCESS;
+ }
}
-BT::NodeStatus IriBTBasicNodes::async_is_stop_tree(void)
+BT::NodeStatus IriBTBasicNodes::compute_distance(BT::TreeNode& self)
{
- if(this->tree_stop)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::RUNNING;
-}
+ ROS_DEBUG("IriBTBasicNodes::compute_distance");
+ BT::Optional<geometry_msgs::PoseStamped> pose1 = self.getInput<geometry_msgs::PoseStamped>("pose1");
+ BT::Optional<geometry_msgs::PoseStamped> pose2 = self.getInput<geometry_msgs::PoseStamped>("pose2");
-BT::NodeStatus IriBTBasicNodes::set_start_tree(void)
-{
- this->tree_start=true;
- return BT::NodeStatus::SUCCESS;
-}
+ 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)");
+ return BT::NodeStatus::FAILURE;
+ }
-BT::NodeStatus IriBTBasicNodes::set_stop_tree(void)
-{
- this->tree_stop=true;
- return BT::NodeStatus::SUCCESS;
-}
+ geometry_msgs::PoseStamped pose1_value=pose1.value();
+ geometry_msgs::PoseStamped pose2_value=pose2.value();
+ double distance;
-BT::NodeStatus IriBTBasicNodes::reset_start_tree(void)
-{
- this->tree_start=false;
+ 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));
+
+ self.setOutput("distance",distance);
return BT::NodeStatus::SUCCESS;
}
-BT::NodeStatus IriBTBasicNodes::reset_stop_tree(void)
+BT::NodeStatus IriBTBasicNodes::compare_bigger(BT::TreeNode& self)
{
- this->tree_stop=false;
- return BT::NodeStatus::SUCCESS;
+ ROS_DEBUG("IriBTBasicNodes::compare_bigger");
+ BT::Optional<double> value1 = self.getInput<double>("value1");
+ BT::Optional<double> value2 = self.getInput<double>("value2");
+
+ if((!value1) || (!value2))
+ {
+ ROS_ERROR("IriBTBasicNodes::compare_bigger-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double val1=value1.value();
+ double val2=value2.value();
+
+ if(val1>val2)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
}
-void IriBTBasicNodes::start_tree(void)
+BT::NodeStatus IriBTBasicNodes::compare_smaller(BT::TreeNode& self)
{
- ROS_DEBUG("IriBTBasicNodes: start_tree()");
- this->tree_start=true;
+ ROS_DEBUG("IriBTBasicNodes::compare_smaller");
+ BT::Optional<double> value1 = self.getInput<double>("value1");
+ BT::Optional<double> value2 = self.getInput<double>("value2");
+
+ if((!value1) || (!value2))
+ {
+ ROS_ERROR("IriBTBasicNodes::compare_smaller-> Incorrect or missing input. It needs the following input ports: value1 (double) value2 (double)");
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double val1=value1.value();
+ double val2=value2.value();
+
+ if(val1<val2)
+ return BT::NodeStatus::SUCCESS;
+ else
+ return BT::NodeStatus::FAILURE;
}
-void IriBTBasicNodes::stop_tree(void)
+BT::NodeStatus IriBTBasicNodes::RUNNING()
{
- ROS_DEBUG("IriBTBasicNodes: stop_tree()");
- this->tree_stop=true;
+ return BT::NodeStatus::RUNNING;
}
--
GitLab