From e44b051f5350b59dee140b8617f06f3042727bba Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 9 Jan 2025 12:11:58 +0100
Subject: [PATCH] Solved some bugs:   * some operations didn't use the second
 value (value2)   * the histeresis was not initialized

---
 src/arithmetic_bt_helpers.cpp | 60 +++++++++++++++++------------------
 1 file changed, 30 insertions(+), 30 deletions(-)

diff --git a/src/arithmetic_bt_helpers.cpp b/src/arithmetic_bt_helpers.cpp
index 76fb096..5a3e8d2 100644
--- a/src/arithmetic_bt_helpers.cpp
+++ b/src/arithmetic_bt_helpers.cpp
@@ -257,23 +257,23 @@ BT::NodeStatus ArithmeticBTHelpers::compare_bigger_hist(BT::TreeNode& self)
 
   //get second operand
   if(self.getInput<double>("value2_double"))
-    value1=self.getInput<double>("value2_double").value();
+    value2=self.getInput<double>("value2_double").value();
   else if(self.getInput<float>("value2_float"))
-    value1=self.getInput<float>("value2_float").value();
+    value2=self.getInput<float>("value2_float").value();
   else if(self.getInput<unsigned char>("value2_uchar"))
-    value1=self.getInput<unsigned char>("value2_uchar").value();
+    value2=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();
+    value2=self.getInput<unsigned short int>("value2_ushort").value();
   else if(self.getInput<unsigned int>("value2_uint"))
-    value1=self.getInput<unsigned int>("value2_uint").value();
+    value2=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();
+    value2=self.getInput<unsigned long int>("value2_ulong").value();
   else if(self.getInput<short int>("value2_short"))
-    value1=self.getInput<short int>("value2_short").value();
+    value2=self.getInput<short int>("value2_short").value();
   else if(self.getInput<int>("value2_int"))
-    value1=self.getInput<int>("value2_int").value();
+    value2=self.getInput<int>("value2_int").value();
   else if(self.getInput<long int>("value2_long"))
-    value1=self.getInput<long int>("value2_long").value();
+    value2=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)");
@@ -288,13 +288,13 @@ BT::NodeStatus ArithmeticBTHelpers::compare_bigger_hist(BT::TreeNode& self)
     return BT::NodeStatus::FAILURE;
   }
   if(self.getInput<bool>("current_state"))
-    hist=self.getInput<bool>("current_state").value();
+    current_state=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;
   }
-
+  std::cout << "Current state: " << current_state << " value1: " << value1 << "  value2: " << value2 << "hist: " << hist << std::endl;
   if(current_state)
   {
     if(value1<(value2-hist))
@@ -355,23 +355,23 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller_hist(BT::TreeNode& self)
   }
   //get second operand
   if(self.getInput<double>("value2_double"))
-    value1=self.getInput<double>("value2_double").value();
+    value2=self.getInput<double>("value2_double").value();
   else if(self.getInput<float>("value2_float"))
-    value1=self.getInput<float>("value2_float").value();
+    value2=self.getInput<float>("value2_float").value();
   else if(self.getInput<unsigned char>("value2_uchar"))
-    value1=self.getInput<unsigned char>("value2_uchar").value();
+    value2=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();
+    value2=self.getInput<unsigned short int>("value2_ushort").value();
   else if(self.getInput<unsigned int>("value2_uint"))
-    value1=self.getInput<unsigned int>("value2_uint").value();
+    value2=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();
+    value2=self.getInput<unsigned long int>("value2_ulong").value();
   else if(self.getInput<short int>("value2_short"))
-    value1=self.getInput<short int>("value2_short").value();
+    value2=self.getInput<short int>("value2_short").value();
   else if(self.getInput<int>("value2_int"))
-    value1=self.getInput<int>("value2_int").value();
+    value2=self.getInput<int>("value2_int").value();
   else if(self.getInput<long int>("value2_long"))
-    value1=self.getInput<long int>("value2_long").value();
+    value2=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)");
@@ -386,7 +386,7 @@ BT::NodeStatus ArithmeticBTHelpers::compare_smaller_hist(BT::TreeNode& self)
     return BT::NodeStatus::FAILURE;
   }
   if(self.getInput<bool>("current_state"))
-    hist=self.getInput<bool>("current_state").value();
+    current_state=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)");
@@ -512,23 +512,23 @@ BT::NodeStatus ArithmeticBTHelpers::compare_different(BT::TreeNode& self)
   }
   //get second operand
   if(self.getInput<double>("value2_double"))
-    value1=self.getInput<double>("value2_double").value();
+    value2=self.getInput<double>("value2_double").value();
   else if(self.getInput<float>("value2_float"))
-    value1=self.getInput<float>("value2_float").value();
+    value2=self.getInput<float>("value2_float").value();
   else if(self.getInput<unsigned char>("value2_uchar"))
-    value1=self.getInput<unsigned char>("value2_uchar").value();
+    value2=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();
+    value2=self.getInput<unsigned short int>("value2_ushort").value();
   else if(self.getInput<unsigned int>("value2_uint"))
-    value1=self.getInput<unsigned int>("value2_uint").value();
+    value2=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();
+    value2=self.getInput<unsigned long int>("value2_ulong").value();
   else if(self.getInput<short int>("value2_short"))
-    value1=self.getInput<short int>("value2_short").value();
+    value2=self.getInput<short int>("value2_short").value();
   else if(self.getInput<int>("value2_int"))
-    value1=self.getInput<int>("value2_int").value();
+    value2=self.getInput<int>("value2_int").value();
   else if(self.getInput<long int>("value2_long"))
-    value1=self.getInput<long int>("value2_long").value();
+    value2=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)");
-- 
GitLab