diff --git a/include/iri_base_bt_client/iri_bt_dyn_reconf.h b/include/iri_base_bt_client/iri_bt_dyn_reconf.h index e577ac96d02b0e272a3cc9ccd5b4c2a617f6e6fa..136c91d8bebc47ba80293f4001a6041495bba7c7 100644 --- a/include/iri_base_bt_client/iri_bt_dyn_reconf.h +++ b/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; } diff --git a/src/common_bt_helpers.cpp b/src/common_bt_helpers.cpp index 406fcf44bcbd411eb729950be4ae0b3f68bef744..02e9322ec5d249ec6dea684cd33d162cce49383b 100644 --- a/src/common_bt_helpers.cpp +++ b/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);