Added a function to cast a numeric value to any type.

include/iri_base_bt_client/arithmetic_bt_helpers.h

@@ -69,6 +69,8 @@ class ArithmeticBTHelpers
     BT::NodeStatus add(BT::TreeNode& self);
 
     BT::NodeStatus substract(BT::TreeNode& self);
+
+    BT::NodeStatus cast(BT::TreeNode& self);
 };
 
 #endif

src/arithmetic_bt_helpers.cpp

@@ -17,6 +17,16 @@ 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 cast_ports = {BT::InputPort<double>("value1_double"),BT::OutputPort<double>("value2_double"),
+                              BT::InputPort<float>("value1_float"),BT::OutputPort<float>("value2_float"),
+                              BT::InputPort<unsigned char>("value1_uchar"),BT::OutputPort<unsigned char>("value2_uchar"),
+                              BT::InputPort<unsigned short int>("value1_ushort"),BT::OutputPort<unsigned short int>("value2_double"),
+                              BT::InputPort<unsigned int>("value1_uint"),BT::OutputPort<unsigned int>("value2_uint"),
+                              BT::InputPort<unsigned long int>("value1_ulong"),BT::OutputPort<unsigned long int>("value2_ulong"),
+                              BT::InputPort<short int>("value1_short"),BT::OutputPort<short int>("value2_short"),
+                              BT::InputPort<int>("value1_int"),BT::OutputPort<int>("value2_int"),
+                              BT::InputPort<long int>("value1_long"),BT::OutputPort<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"),
@@ -56,6 +66,7 @@ void ArithmeticBTHelpers::init(IriBehaviorTreeFactory &factory)
   factory.registerSimpleAction("product", std::bind(&ArithmeticBTHelpers::product, this, std::placeholders::_1),operation_ports);
   factory.registerSimpleAction("add", std::bind(&ArithmeticBTHelpers::add, this, std::placeholders::_1),operation_ports);
   factory.registerSimpleAction("substract", std::bind(&ArithmeticBTHelpers::substract, this, std::placeholders::_1),operation_ports);
+  factory.registerSimpleAction("cast", std::bind(&ArithmeticBTHelpers::cast, this, std::placeholders::_1),cast_ports);
 }
 
 BT::NodeStatus ArithmeticBTHelpers::is_variable_true(BT::TreeNode& self)
@@ -926,3 +937,45 @@ BT::NodeStatus ArithmeticBTHelpers::substract(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus ArithmeticBTHelpers::cast(BT::TreeNode& self)
+{
+  double value1,value2;
+  ROS_DEBUG("ArithmeticBTHelpers::cast");
+
+  // 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::cast (" << 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;    
+  }
+  self.setOutput("value2_double",value1);
+  self.setOutput("value2_float",(float)value1);
+  self.setOutput("value2_uchar",(unsigned char)value1);
+  self.setOutput("value2_ushort",(unsigned short int)value1);
+  self.setOutput("value2_uint",(unsigned int)value1);
+  self.setOutput("value2_ulong",(unsigned long int)value1);
+  self.setOutput("value2_char",(char)value1);
+  self.setOutput("value2_short",(short int)value1);
+  self.setOutput("value2_int",(int)value1);
+  self.setOutput("value2_long",(long int)value1);
+
+  return BT::NodeStatus::SUCCESS;
+}