From 080a16371e61cf16fcecce1f094d083e9711eef8 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Wed, 9 Oct 2024 08:08:09 +0200 Subject: [PATCH] Added some new BT actions to the common and arithmetic helpers. --- .../arithmetic_bt_helpers.h | 32 +- .../iri_base_bt_client/common_bt_helpers.h | 2 + src/arithmetic_bt_helpers.cpp | 313 ++++++++++++++++-- src/common_bt_helpers.cpp | 22 ++ 4 files changed, 322 insertions(+), 47 deletions(-) diff --git a/include/iri_base_bt_client/arithmetic_bt_helpers.h b/include/iri_base_bt_client/arithmetic_bt_helpers.h index d48a3c9..31e2a8d 100644 --- a/include/iri_base_bt_client/arithmetic_bt_helpers.h +++ b/include/iri_base_bt_client/arithmetic_bt_helpers.h @@ -46,42 +46,22 @@ class ArithmeticBTHelpers */ BT::NodeStatus is_variable_false(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); - /** - * \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); BT::NodeStatus compare_equal(BT::TreeNode& self); + BT::NodeStatus compare_different(BT::TreeNode& self); + BT::NodeStatus compare_bigger_equal(BT::TreeNode& self); BT::NodeStatus compare_smaller_equal(BT::TreeNode& self); + BT::NodeStatus compare_bigger_hist(BT::TreeNode& self); + + BT::NodeStatus compare_smaller_hist(BT::TreeNode& self); + BT::NodeStatus division(BT::TreeNode& self); BT::NodeStatus product(BT::TreeNode& self); diff --git a/include/iri_base_bt_client/common_bt_helpers.h b/include/iri_base_bt_client/common_bt_helpers.h index 3b10a48..d882b7a 100644 --- a/include/iri_base_bt_client/common_bt_helpers.h +++ b/include/iri_base_bt_client/common_bt_helpers.h @@ -54,6 +54,8 @@ class CommonBTHelpers */ BT::NodeStatus print_msg(BT::TreeNode& _self); + BT::NodeStatus concatenate_string(BT::TreeNode& _self); + /** * \brief Action to create (if necessary) and to start a timeout * diff --git a/src/arithmetic_bt_helpers.cpp b/src/arithmetic_bt_helpers.cpp index a1d1e44..01c9d23 100644 --- a/src/arithmetic_bt_helpers.cpp +++ b/src/arithmetic_bt_helpers.cpp @@ -17,6 +17,17 @@ void ArithmeticBTHelpers::init(IriBehaviorTreeFactory &factory) BT::InputPort<short int>("value1_short"),BT::InputPort<short int>("value2_short"), BT::InputPort<int>("value1_int"),BT::InputPort<int>("value2_int"), BT::InputPort<long int>("value1_long"),BT::InputPort<long int>("value2_long")}; + BT::PortsList compare_hist_ports = {BT::InputPort<double>("value1_double"),BT::InputPort<double>("value2_double"), + BT::InputPort<float>("value1_float"),BT::InputPort<float>("value2_float"), + BT::InputPort<unsigned char>("value1_uchar"),BT::InputPort<unsigned char>("value2_uchar"), + BT::InputPort<unsigned short int>("value1_ushort"),BT::InputPort<unsigned short int>("value2_double"), + BT::InputPort<unsigned int>("value1_uint"),BT::InputPort<unsigned int>("value2_uint"), + BT::InputPort<unsigned long int>("value1_ulong"),BT::InputPort<unsigned long int>("value2_ulong"), + BT::InputPort<short int>("value1_short"),BT::InputPort<short int>("value2_short"), + BT::InputPort<int>("value1_int"),BT::InputPort<int>("value2_int"), + BT::InputPort<long int>("value1_long"),BT::InputPort<long int>("value2_long"), + BT::InputPort<double>("hist"),BT::BidirectionalPort<bool>("current_state")}; + BT::PortsList operation_ports = {BT::InputPort<double>("value1_double"),BT::InputPort<double>("value2_double"), BT::InputPort<float>("value1_float"),BT::InputPort<float>("value2_float"), BT::InputPort<unsigned char>("value1_uchar"),BT::InputPort<unsigned char>("value2_uchar"), @@ -26,7 +37,7 @@ void ArithmeticBTHelpers::init(IriBehaviorTreeFactory &factory) BT::InputPort<short int>("value1_short"),BT::InputPort<short int>("value2_short"), BT::InputPort<int>("value1_int"),BT::InputPort<int>("value2_int"), BT::InputPort<long int>("value1_long"),BT::InputPort<long int>("value2_long"), - BT::InputPort<double>("result_double"),BT::InputPort<float>("result_float"),BT::InputPort<unsigned char>("result_uchar"),BT::InputPort<unsigned short int>("result_ushort"),BT::InputPort<unsigned int>("result_uint"),BT::InputPort<unsigned long int>("result_ulong"),BT::InputPort<char>("result_char"),BT::InputPort<short int>("result_short"),BT::InputPort<int>("result_int"),BT::InputPort<long int>("result_long")}; + BT::OutputPort<double>("result_double"),BT::OutputPort<float>("result_float"),BT::OutputPort<unsigned char>("result_uchar"),BT::OutputPort<unsigned short int>("result_ushort"),BT::OutputPort<unsigned int>("result_uint"),BT::OutputPort<unsigned long int>("result_ulong"),BT::OutputPort<char>("result_char"),BT::OutputPort<short int>("result_short"),BT::OutputPort<int>("result_int"),BT::OutputPort<long int>("result_long")}; //Registration of conditions factory.registerSimpleCondition("is_variable_true", std::bind(&ArithmeticBTHelpers::is_variable_true, this, std::placeholders::_1),check_variable_ports); @@ -34,7 +45,10 @@ void ArithmeticBTHelpers::init(IriBehaviorTreeFactory &factory) factory.registerSimpleAction("compare_bigger", std::bind(&ArithmeticBTHelpers::compare_bigger, this, std::placeholders::_1),compare_ports); factory.registerSimpleAction("compare_smaller", std::bind(&ArithmeticBTHelpers::compare_smaller, this, std::placeholders::_1),compare_ports); + factory.registerSimpleAction("compare_bigger_hist", std::bind(&ArithmeticBTHelpers::compare_bigger_hist, this, std::placeholders::_1),compare_hist_ports); + factory.registerSimpleAction("compare_smaller_hist", std::bind(&ArithmeticBTHelpers::compare_smaller_hist, this, std::placeholders::_1),compare_hist_ports); factory.registerSimpleAction("compare_equal", std::bind(&ArithmeticBTHelpers::compare_equal, this, std::placeholders::_1),compare_ports); + factory.registerSimpleAction("compare_different", std::bind(&ArithmeticBTHelpers::compare_different, this, std::placeholders::_1),compare_ports); factory.registerSimpleAction("compare_bigger_equal", std::bind(&ArithmeticBTHelpers::compare_bigger_equal, this, std::placeholders::_1),compare_ports); factory.registerSimpleAction("compare_smaller_equal", std::bind(&ArithmeticBTHelpers::compare_smaller_equal, this, std::placeholders::_1),compare_ports); @@ -165,7 +179,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -189,7 +203,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -199,6 +213,203 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller(BT::TreeNode& self) return BT::NodeStatus::FAILURE; } +BT::NodeStatus ArithmeticBTHelpers::compare_bigger_hist(BT::TreeNode& self) +{ + double value1,value2,hist; + bool current_state; + ROS_DEBUG("ArithmeticBTHelpers::compare_bigger"); + + // get first operand + if(self.getInput<double>("value1_double")) + value1=self.getInput<double>("value1_double").value(); + else if(self.getInput<float>("value1_float")) + value1=self.getInput<float>("value1_float").value(); + else if(self.getInput<unsigned char>("value1_uchar")) + value1=self.getInput<unsigned char>("value1_uchar").value(); + else if(self.getInput<unsigned short int>("value1_ushort")) + value1=self.getInput<unsigned short int>("value1_ushort").value(); + else if(self.getInput<unsigned int>("value1_uint")) + value1=self.getInput<unsigned int>("value1_uint").value(); + else if(self.getInput<unsigned long int>("value1_ulong")) + value1=self.getInput<unsigned long int>("value1_ulong").value(); + else if(self.getInput<short int>("value1_short")) + value1=self.getInput<short int>("value1_short").value(); + else if(self.getInput<int>("value1_int")) + value1=self.getInput<int>("value1_int").value(); + else if(self.getInput<long int>("value1_long")) + value1=self.getInput<long int>("value1_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + + //get second operand + if(self.getInput<double>("value2_double")) + value1=self.getInput<double>("value2_double").value(); + else if(self.getInput<float>("value2_float")) + value1=self.getInput<float>("value2_float").value(); + else if(self.getInput<unsigned char>("value2_uchar")) + value1=self.getInput<unsigned char>("value2_uchar").value(); + else if(self.getInput<unsigned short int>("value2_ushort")) + value1=self.getInput<unsigned short int>("value2_ushort").value(); + else if(self.getInput<unsigned int>("value2_uint")) + value1=self.getInput<unsigned int>("value2_uint").value(); + else if(self.getInput<unsigned long int>("value2_ulong")) + value1=self.getInput<unsigned long int>("value2_ulong").value(); + else if(self.getInput<short int>("value2_short")) + value1=self.getInput<short int>("value2_short").value(); + else if(self.getInput<int>("value2_int")) + value1=self.getInput<int>("value2_int").value(); + else if(self.getInput<long int>("value2_long")) + value1=self.getInput<long int>("value2_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + + if(self.getInput<double>("hist")) + hist=self.getInput<double>("hist").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + if(self.getInput<bool>("current_state")) + hist=self.getInput<bool>("current_state").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + + if(current_state) + { + if(value1<(value2-hist)) + { + self.setOutput("current_state", false); + return BT::NodeStatus::FAILURE; + } + else + { + self.setOutput("current_state", true); + return BT::NodeStatus::SUCCESS; + } + } + else + { + if(value1>(value2+hist)) + { + self.setOutput("current_state", true); + return BT::NodeStatus::SUCCESS; + } + else + { + self.setOutput("current_state", false); + return BT::NodeStatus::FAILURE; + } + } +} + +BT::NodeStatus ArithmeticBTHelpers::compare_smaller_hist(BT::TreeNode& self) +{ + double value1,value2,hist; + bool current_state; + ROS_DEBUG("ArithmeticBTHelpers::compare_smaller"); + + // get first operand + if(self.getInput<double>("value1_double")) + value1=self.getInput<double>("value1_double").value(); + else if(self.getInput<float>("value1_float")) + value1=self.getInput<float>("value1_float").value(); + else if(self.getInput<unsigned char>("value1_uchar")) + value1=self.getInput<unsigned char>("value1_uchar").value(); + else if(self.getInput<unsigned short int>("value1_ushort")) + value1=self.getInput<unsigned short int>("value1_ushort").value(); + else if(self.getInput<unsigned int>("value1_uint")) + value1=self.getInput<unsigned int>("value1_uint").value(); + else if(self.getInput<unsigned long int>("value1_ulong")) + value1=self.getInput<unsigned long int>("value1_ulong").value(); + else if(self.getInput<short int>("value1_short")) + value1=self.getInput<short int>("value1_short").value(); + else if(self.getInput<int>("value1_int")) + value1=self.getInput<int>("value1_int").value(); + else if(self.getInput<long int>("value1_long")) + value1=self.getInput<long int>("value1_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + //get second operand + if(self.getInput<double>("value2_double")) + value1=self.getInput<double>("value2_double").value(); + else if(self.getInput<float>("value2_float")) + value1=self.getInput<float>("value2_float").value(); + else if(self.getInput<unsigned char>("value2_uchar")) + value1=self.getInput<unsigned char>("value2_uchar").value(); + else if(self.getInput<unsigned short int>("value2_ushort")) + value1=self.getInput<unsigned short int>("value2_ushort").value(); + else if(self.getInput<unsigned int>("value2_uint")) + value1=self.getInput<unsigned int>("value2_uint").value(); + else if(self.getInput<unsigned long int>("value2_ulong")) + value1=self.getInput<unsigned long int>("value2_ulong").value(); + else if(self.getInput<short int>("value2_short")) + value1=self.getInput<short int>("value2_short").value(); + else if(self.getInput<int>("value2_int")) + value1=self.getInput<int>("value2_int").value(); + else if(self.getInput<long int>("value2_long")) + value1=self.getInput<long int>("value2_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + + if(self.getInput<double>("hist")) + hist=self.getInput<double>("hist").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + if(self.getInput<bool>("current_state")) + hist=self.getInput<bool>("current_state").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) hist (double) current_state (bool)"); + return BT::NodeStatus::FAILURE; + } + + if(current_state) + { + if(value1>(value2+hist)) + { + self.setOutput("current_state", false); + return BT::NodeStatus::FAILURE; + } + else + { + self.setOutput("current_state", true); + return BT::NodeStatus::SUCCESS; + } + } + else + { + if(value1<(value2-hist)) + { + self.setOutput("current_state", true); + return BT::NodeStatus::SUCCESS; + } + else + { + self.setOutput("current_state", false); + return BT::NodeStatus::FAILURE; + } + } +} + BT::NodeStatus ArithmeticBTHelpers::compare_equal(BT::TreeNode& self) { double value1,value2; @@ -225,7 +436,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_equal(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_equal (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -249,7 +460,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_equal(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_equal (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -259,6 +470,66 @@ BT::NodeStatus ArithmeticBTHelpers::compare_equal(BT::TreeNode& self) return BT::NodeStatus::FAILURE; } +BT::NodeStatus ArithmeticBTHelpers::compare_different(BT::TreeNode& self) +{ + double value1,value2; + ROS_DEBUG("ArithmeticBTHelpers::compare_different"); + + // get first operand + if(self.getInput<double>("value1_double")) + value1=self.getInput<double>("value1_double").value(); + else if(self.getInput<float>("value1_float")) + value1=self.getInput<float>("value1_float").value(); + else if(self.getInput<unsigned char>("value1_uchar")) + value1=self.getInput<unsigned char>("value1_uchar").value(); + else if(self.getInput<unsigned short int>("value1_ushort")) + value1=self.getInput<unsigned short int>("value1_ushort").value(); + else if(self.getInput<unsigned int>("value1_uint")) + value1=self.getInput<unsigned int>("value1_uint").value(); + else if(self.getInput<unsigned long int>("value1_ulong")) + value1=self.getInput<unsigned long int>("value1_ulong").value(); + else if(self.getInput<short int>("value1_short")) + value1=self.getInput<short int>("value1_short").value(); + else if(self.getInput<int>("value1_int")) + value1=self.getInput<int>("value1_int").value(); + else if(self.getInput<long int>("value1_long")) + value1=self.getInput<long int>("value1_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_different (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + return BT::NodeStatus::FAILURE; + } + //get second operand + if(self.getInput<double>("value2_double")) + value1=self.getInput<double>("value2_double").value(); + else if(self.getInput<float>("value2_float")) + value1=self.getInput<float>("value2_float").value(); + else if(self.getInput<unsigned char>("value2_uchar")) + value1=self.getInput<unsigned char>("value2_uchar").value(); + else if(self.getInput<unsigned short int>("value2_ushort")) + value1=self.getInput<unsigned short int>("value2_ushort").value(); + else if(self.getInput<unsigned int>("value2_uint")) + value1=self.getInput<unsigned int>("value2_uint").value(); + else if(self.getInput<unsigned long int>("value2_ulong")) + value1=self.getInput<unsigned long int>("value2_ulong").value(); + else if(self.getInput<short int>("value2_short")) + value1=self.getInput<short int>("value2_short").value(); + else if(self.getInput<int>("value2_int")) + value1=self.getInput<int>("value2_int").value(); + else if(self.getInput<long int>("value2_long")) + value1=self.getInput<long int>("value2_long").value(); + else + { + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_different (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + return BT::NodeStatus::FAILURE; + } + + if(value1!=value2) + return BT::NodeStatus::SUCCESS; + else + return BT::NodeStatus::FAILURE; +} + BT::NodeStatus ArithmeticBTHelpers::compare_bigger_equal(BT::TreeNode& self) { double value1,value2; @@ -285,7 +556,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_bigger_equal(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger_equal (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -309,7 +580,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_bigger_equal(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger_equal (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -345,7 +616,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller_equal(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -369,7 +640,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller_equal(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_smaller (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -382,7 +653,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller_equal(BT::TreeNode& self) BT::NodeStatus ArithmeticBTHelpers::division(BT::TreeNode& self) { double value1,value2,result; - ROS_DEBUG("ArithmeticBTHelpers::compare_smaller"); + ROS_DEBUG("ArithmeticBTHelpers::division"); // get first operand if(self.getInput<double>("value1_double")) @@ -405,7 +676,7 @@ BT::NodeStatus ArithmeticBTHelpers::division(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::division (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -429,7 +700,7 @@ BT::NodeStatus ArithmeticBTHelpers::division(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::division (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -451,7 +722,7 @@ BT::NodeStatus ArithmeticBTHelpers::division(BT::TreeNode& self) BT::NodeStatus ArithmeticBTHelpers::product(BT::TreeNode& self) { double value1,value2,result; - ROS_DEBUG("ArithmeticBTHelpers::compare_smaller"); + ROS_DEBUG("ArithmeticBTHelpers::product"); // get first operand if(self.getInput<double>("value1_double")) @@ -474,7 +745,7 @@ BT::NodeStatus ArithmeticBTHelpers::product(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::product (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -498,7 +769,7 @@ BT::NodeStatus ArithmeticBTHelpers::product(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::product (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -520,7 +791,7 @@ BT::NodeStatus ArithmeticBTHelpers::product(BT::TreeNode& self) BT::NodeStatus ArithmeticBTHelpers::add(BT::TreeNode& self) { double value1,value2,result; - ROS_DEBUG("ArithmeticBTHelpers::compare_smaller"); + ROS_DEBUG("ArithmeticBTHelpers::add"); // get first operand if(self.getInput<double>("value1_double")) @@ -543,7 +814,7 @@ BT::NodeStatus ArithmeticBTHelpers::add(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::add (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -567,7 +838,7 @@ BT::NodeStatus ArithmeticBTHelpers::add(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::add (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } @@ -589,7 +860,7 @@ BT::NodeStatus ArithmeticBTHelpers::add(BT::TreeNode& self) BT::NodeStatus ArithmeticBTHelpers::substract(BT::TreeNode& self) { double value1,value2,result; - ROS_DEBUG("ArithmeticBTHelpers::compare_smaller"); + ROS_DEBUG("ArithmeticBTHelpers::substract"); // get first operand if(self.getInput<double>("value1_double")) @@ -612,7 +883,7 @@ BT::NodeStatus ArithmeticBTHelpers::substract(BT::TreeNode& self) value1=self.getInput<long int>("value1_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::substract (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } //get second operand @@ -636,7 +907,7 @@ BT::NodeStatus ArithmeticBTHelpers::substract(BT::TreeNode& self) value1=self.getInput<long int>("value2_long").value(); else { - ROS_ERROR_STREAM("ArithmeticBTHelpers::compare_bigger (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); + ROS_ERROR_STREAM("ArithmeticBTHelpers::substract (" << self.name() << ") -> Incorrect or missing input. It needs the following input ports: value1 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int) value2 (double|float|unsigned char|unsigned short int|usigned int|unsigned long int|char|short int|int|long int)"); return BT::NodeStatus::FAILURE; } diff --git a/src/common_bt_helpers.cpp b/src/common_bt_helpers.cpp index 5aad07b..0569cc3 100644 --- a/src/common_bt_helpers.cpp +++ b/src/common_bt_helpers.cpp @@ -8,12 +8,14 @@ CommonBTHelpers::CommonBTHelpers() void CommonBTHelpers::init(IriBehaviorTreeFactory &factory) { BT::PortsList print_msg_ports = {BT::InputPort<std::string>("msg"), BT::InputPort<std::string>("type")}; + BT::PortsList concatenate_ports = {BT::BidirectionalPort<std::string>("string1"), BT::InputPort<std::string>("string2")}; BT::PortsList start_timeout_ports = {BT::InputPort<std::string>("tout_name"), BT::InputPort<double>("duration")}; BT::PortsList stop_timeout_ports = {BT::InputPort<std::string>("tout_name")}; BT::PortsList get_timed_out_ports = {BT::InputPort<std::string>("tout_name"), BT::OutputPort<bool>("timed_out")}; factory.registerIriAsyncAction("RUNNING", std::bind(&CommonBTHelpers::RUNNING, this)); factory.registerSimpleAction("print_msg", std::bind(&CommonBTHelpers::print_msg, this, std::placeholders::_1), print_msg_ports); + factory.registerSimpleAction("concatenate_string", std::bind(&CommonBTHelpers::concatenate_string, this, std::placeholders::_1), concatenate_ports); factory.registerSimpleAction("start_timeout", std::bind(&CommonBTHelpers::start_timeout, this, std::placeholders::_1), start_timeout_ports); factory.registerSimpleAction("stop_timeout", std::bind(&CommonBTHelpers::stop_timeout, this, std::placeholders::_1), stop_timeout_ports); factory.registerSimpleAction("get_timed_out", std::bind(&CommonBTHelpers::get_timed_out, this, std::placeholders::_1), get_timed_out_ports); @@ -52,6 +54,26 @@ BT::NodeStatus CommonBTHelpers::print_msg(BT::TreeNode& _self) return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CommonBTHelpers::concatenate_string(BT::TreeNode& _self) +{ + BT::Optional<std::string> string1_p = _self.getInput<std::string>("string1"); + BT::Optional<std::string> string2_p = _self.getInput<std::string>("string2"); + std::string string1,string2; + + if (!string1_p || !string2_p) + { + ROS_ERROR_STREAM("CommonBTHelpers::concatenate_string (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:" << " string1 (sdt::string) string2 (std::string)"); + return BT::NodeStatus::FAILURE; + } + string1 = string1_p.value(); + string2 = string2_p.value(); + + string1=string1+string2; + _self.setOutput("string1", string1); + + return BT::NodeStatus::SUCCESS; +} + BT::NodeStatus CommonBTHelpers::start_timeout(BT::TreeNode& _self) { BT::Optional<std::string> name_p = _self.getInput<std::string>("tout_name"); -- GitLab