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