diff --git a/include/iri_base_bt_client/common_bt_helpers.h b/include/iri_base_bt_client/common_bt_helpers.h index d882b7ac42ccadfb011a65b2b3b1f15ca1e8fe03..56bebb06d17b45d2138d287d8afddccd65ae5acb 100644 --- a/include/iri_base_bt_client/common_bt_helpers.h +++ b/include/iri_base_bt_client/common_bt_helpers.h @@ -54,6 +54,28 @@ class CommonBTHelpers */ BT::NodeStatus print_msg(BT::TreeNode& _self); + /** + * \brief Function to write in terminal the value of a port given its name and value. + * + * It has the following input ports: + * + * msg (std::string): port name to be printed. + * + * value_str (std::string): value of the port if it is a string + * + * value_double (double): value of the port if it is a double + * + * value_int (int): value of the port if it is an int + * + * \param _self node with the required ports: + * + * \return a BT:NodeStatus indicating whether the request has been + * successfull (BT::NodeStatus::SUCCESS) or not (BT::NodeStatus::FAILURE). + * If inputs are missing or incorrect it also returns BT::NodeStatus::FAILURE. + * + */ + BT::NodeStatus print_port(BT::TreeNode& _self); + BT::NodeStatus concatenate_string(BT::TreeNode& _self); /** diff --git a/src/common_bt_helpers.cpp b/src/common_bt_helpers.cpp index 0569cc3cf77d4143e5ad118b251eca9ecb59a26c..406fcf44bcbd411eb729950be4ae0b3f68bef744 100644 --- a/src/common_bt_helpers.cpp +++ b/src/common_bt_helpers.cpp @@ -8,6 +8,10 @@ CommonBTHelpers::CommonBTHelpers() void CommonBTHelpers::init(IriBehaviorTreeFactory &factory) { BT::PortsList print_msg_ports = {BT::InputPort<std::string>("msg"), BT::InputPort<std::string>("type")}; + 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::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")}; @@ -15,6 +19,7 @@ void CommonBTHelpers::init(IriBehaviorTreeFactory &factory) factory.registerIriAsyncAction("RUNNING", std::bind(&CommonBTHelpers::RUNNING, this)); factory.registerSimpleAction("print_msg", std::bind(&CommonBTHelpers::print_msg, this, std::placeholders::_1), print_msg_ports); + factory.registerSimpleAction("print_port", std::bind(&CommonBTHelpers::print_port, this, std::placeholders::_1), print_port_ports); factory.registerSimpleAction("concatenate_string", std::bind(&CommonBTHelpers::concatenate_string, this, std::placeholders::_1), concatenate_ports); factory.registerSimpleAction("start_timeout", std::bind(&CommonBTHelpers::start_timeout, this, std::placeholders::_1), start_timeout_ports); factory.registerSimpleAction("stop_timeout", std::bind(&CommonBTHelpers::stop_timeout, this, std::placeholders::_1), stop_timeout_ports); @@ -54,6 +59,48 @@ BT::NodeStatus CommonBTHelpers::print_msg(BT::TreeNode& _self) return BT::NodeStatus::SUCCESS; } +BT::NodeStatus CommonBTHelpers::print_port(BT::TreeNode& _self) +{ + BT::Optional<std::string> port_name_p = _self.getInput<std::string>("port_name"); + 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"); + std::string port_name; + std::string value_str; + double value_double; + int value_int; + std::string msg; + std::string msg_value; + + if (!port_name_p || !(value_str_p || value_double_p || value_int_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"); + return BT::NodeStatus::FAILURE; + } + + port_name = port_name_p.value(); + if(value_str_p) + { + msg_value = value_str_p.value(); + } + else if(value_double_p) + { + msg_value = std::to_string(value_double_p.value()); + + } + else if(value_int_p) + { + msg_value = std::to_string(value_int_p.value()); + } + + msg = port_name + ": " + msg_value; + ROS_INFO_STREAM(msg); + + return BT::NodeStatus::SUCCESS; +} + + BT::NodeStatus CommonBTHelpers::concatenate_string(BT::TreeNode& _self) { BT::Optional<std::string> string1_p = _self.getInput<std::string>("string1");