diff --git a/src/arithmetic_bt_helpers.cpp b/src/arithmetic_bt_helpers.cpp
index 76fb09685fddf7065ce35ac79e669ee7fc9076e7..5a3e8d20ad1c0460a4a2e08ad823d25063f0ffbe 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)");