Fix ROS_ERROR msg for missing ports in get_dyn_reconf_* functions. Add...

Fix ROS_ERROR msg for missing ports in get_dyn_reconf_* functions. Add unsigned int type for print_port action

include/iri_base_bt_client/iri_bt_dyn_reconf.h

@@ -197,7 +197,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_string(BT::TreeNode& self)
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_string -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_string -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -222,7 +222,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_bool(BT::TreeNode& self)
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_bool -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_bool -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -247,7 +247,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_integer(BT::TreeNode& self
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_integer -> Incorrect or missing input. It needs the following input ports: name (std::string) value (int)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_integer -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -272,7 +272,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_unsigned_integer(BT::TreeN
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_unsigned_integer -> Incorrect or missing input. It needs the following input ports: name (std::string) value (int)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_unsigned_integer -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -297,7 +297,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double(BT::TreeNode& self)
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::string)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -322,7 +322,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double_vector(BT::TreeNode
 
   if ((!config) || (!name))
   {
-    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double_vector -> Incorrect or missing input. It needs the following input ports: name (std::string) value (std::vector<double>)");
+    ROS_ERROR("IriBTDynReconf::get_dyn_reconf_double_vector -> Incorrect or missing input. It needs the following input ports: dyn_config (Config *) and param_name (std::string)");
     return BT::NodeStatus::FAILURE;
   }
 

src/common_bt_helpers.cpp

@@ -11,7 +11,8 @@ void CommonBTHelpers::init(IriBehaviorTreeFactory &factory)
   BT::PortsList print_port_ports = {BT::InputPort<std::string>("port_name"), 
                                     BT::InputPort<std::string>("value_str"), 
                                     BT::InputPort<double>("value_double"),
-                                    BT::InputPort<int>("value_int")  };
+                                    BT::InputPort<int>("value_int"),
+                                    BT::InputPort<unsigned int>("value_uint")  };
   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")};
@@ -65,17 +66,19 @@ BT::NodeStatus CommonBTHelpers::print_port(BT::TreeNode& _self)
   BT::Optional<std::string> value_str_p    = _self.getInput<std::string>("value_str");
   BT::Optional<double> value_double_p      = _self.getInput<double>("value_double");
   BT::Optional<int> value_int_p            = _self.getInput<int>("value_int");
+  BT::Optional<unsigned int> value_uint_p  = _self.getInput<unsigned int>("value_uint");
   std::string port_name;
   std::string value_str;
   double value_double;
   int value_int;
+  unsigned int value_uint;
   std::string msg;
   std::string msg_value;
 
-  if (!port_name_p || !(value_str_p || value_double_p || value_int_p))
+  if (!port_name_p || !(value_str_p || value_double_p || value_int_p || value_uint_p))
   {
     ROS_ERROR_STREAM("common_helpers::print_port (" << _self.name() << ") -> Incorrect or missing input. It needs the following input ports:"
-                      << " port_name (sdt::string)" << ", one of value_str, value_double, value_int");
+                      << " port_name (sdt::string)" << ", one of value_str, value_double, value_int, value_uint");
     return BT::NodeStatus::FAILURE;
   }
 
@@ -93,6 +96,10 @@ BT::NodeStatus CommonBTHelpers::print_port(BT::TreeNode& _self)
   {
     msg_value = std::to_string(value_int_p.value());
   }
+  else if(value_uint_p)
+  {
+    msg_value = std::to_string(value_uint_p.value());
+  }
 
   msg = port_name + ": " + msg_value;
   ROS_INFO_STREAM(msg);