diff --git a/include/iri_base_bt_client/iri_base_bt_client.h b/include/iri_base_bt_client/iri_base_bt_client.h index 76e540eeaf5fc9845de47c82122e3ff8066b328c..e6e17e1507553e840318c56ebc442e030cc8c88e 100644 --- a/include/iri_base_bt_client/iri_base_bt_client.h +++ b/include/iri_base_bt_client/iri_base_bt_client.h @@ -431,7 +431,7 @@ IriBaseBTClient<ConfigClass>::IriBaseBTClient(const ros::NodeHandle &nh) : // initialize core actions and conditions this->core.init(this->factory); - BT::PortsList get_config_ports = {BT::OutputPort<ConfigClass>("dyn_config")}; + BT::PortsList get_config_ports = {BT::OutputPort<ConfigClass *>("dyn_config")}; this->factory.registerSimpleAction("get_config", std::bind(&IriBaseBTClient<ConfigClass>::get_config, this, std::placeholders::_1),get_config_ports); if(!this->private_node_handle_.getParam("path",this->path)) @@ -807,7 +807,7 @@ BT::NodeStatus IriBaseBTClient<ConfigClass>::get_config(BT::TreeNode& self) { ROS_DEBUG("IriBaseBTClient::get_config"); - self.setOutput("dyn_config", this->config_); + self.setOutput("dyn_config", &this->config_); return BT::NodeStatus::SUCCESS; } 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 40d475054e7675f5ff559621dfa76f7708c2d369..e577ac96d02b0e272a3cc9ccd5b4c2a617f6e6fa 100644 --- a/include/iri_base_bt_client/iri_bt_dyn_reconf.h +++ b/include/iri_base_bt_client/iri_bt_dyn_reconf.h @@ -24,11 +24,11 @@ class IriBTDynReconf void init(IriBehaviorTreeFactory &factory); private: - bool get_string(Config &config, const std::string &name,std::string &value); - bool get_bool(Config &config, const std::string &name, bool &value); - bool get_integer(Config &config, const std::string &name, int &value); - bool get_unsigned_integer(Config &config, const std::string &name, unsigned int &value); - bool get_double(Config &config, const std::string &name, double &value); + bool get_string(Config *config, const std::string &name,std::string &value); + bool get_bool(Config *config, const std::string &name, bool &value); + bool get_integer(Config *config, const std::string &name, int &value); + bool get_unsigned_integer(Config *config, const std::string &name, unsigned int &value); + bool get_double(Config *config, const std::string &name, double &value); // variables to know whether start or stop have been demanded by the user protected: @@ -46,7 +46,7 @@ IriBTDynReconf<Config>::IriBTDynReconf() } template<class Config> -bool IriBTDynReconf<Config>::get_string(Config &config, const std::string &name,std::string &value) +bool IriBTDynReconf<Config>::get_string(Config *config, const std::string &name,std::string &value) { std::vector<typename Config::AbstractParamDescriptionConstPtr> params; boost::any dyn_value; @@ -57,7 +57,7 @@ bool IriBTDynReconf<Config>::get_string(Config &config, const std::string &name, { if((*param)->name==(name)) { - (*param)->getValue(config,dyn_value); + (*param)->getValue(*config,dyn_value); if(dyn_value.type()==typeid(std::string)) { value=boost::any_cast<std::string &>(dyn_value); @@ -71,7 +71,7 @@ bool IriBTDynReconf<Config>::get_string(Config &config, const std::string &name, } template<class Config> -bool IriBTDynReconf<Config>::get_bool(Config &config, const std::string &name, bool &value) +bool IriBTDynReconf<Config>::get_bool(Config *config, const std::string &name, bool &value) { std::vector<typename Config::AbstractParamDescriptionConstPtr> params; boost::any dyn_value; @@ -82,7 +82,7 @@ bool IriBTDynReconf<Config>::get_bool(Config &config, const std::string &name, b { if((*param)->name==(name)) { - (*param)->getValue(config,dyn_value); + (*param)->getValue(*config,dyn_value); if(dyn_value.type()==typeid(bool)) { value=boost::any_cast<bool &>(dyn_value); @@ -96,7 +96,7 @@ bool IriBTDynReconf<Config>::get_bool(Config &config, const std::string &name, b } template<class Config> -bool IriBTDynReconf<Config>::get_integer(Config &config, const std::string &name, int &value) +bool IriBTDynReconf<Config>::get_integer(Config *config, const std::string &name, int &value) { std::vector<typename Config::AbstractParamDescriptionConstPtr> params; boost::any dyn_value; @@ -107,7 +107,7 @@ bool IriBTDynReconf<Config>::get_integer(Config &config, const std::string &name { if((*param)->name==(name)) { - (*param)->getValue(config,dyn_value); + (*param)->getValue(*config,dyn_value); if(dyn_value.type()==typeid(int)) { value=boost::any_cast<int &>(dyn_value); @@ -121,7 +121,7 @@ bool IriBTDynReconf<Config>::get_integer(Config &config, const std::string &name } template<class Config> -bool IriBTDynReconf<Config>::get_unsigned_integer(Config &config, const std::string &name, unsigned int &value) +bool IriBTDynReconf<Config>::get_unsigned_integer(Config *config, const std::string &name, unsigned int &value) { std::vector<typename Config::AbstractParamDescriptionConstPtr> params; boost::any dyn_value; @@ -132,7 +132,7 @@ bool IriBTDynReconf<Config>::get_unsigned_integer(Config &config, const std::str { if((*param)->name==(name)) { - (*param)->getValue(config,dyn_value); + (*param)->getValue(*config,dyn_value); if(dyn_value.type()==typeid(int)) { value=(unsigned int)boost::any_cast<int &>(dyn_value); @@ -146,7 +146,7 @@ bool IriBTDynReconf<Config>::get_unsigned_integer(Config &config, const std::str } template<class Config> -bool IriBTDynReconf<Config>::get_double(Config &config, const std::string &name, double &value) +bool IriBTDynReconf<Config>::get_double(Config *config, const std::string &name, double &value) { std::vector<typename Config::AbstractParamDescriptionConstPtr> params; boost::any dyn_value; @@ -157,7 +157,7 @@ bool IriBTDynReconf<Config>::get_double(Config &config, const std::string &name, { if((*param)->name==(name)) { - (*param)->getValue(config,dyn_value); + (*param)->getValue(*config,dyn_value); if(dyn_value.type()==typeid(double)) { value=boost::any_cast<double &>(dyn_value); @@ -173,12 +173,12 @@ bool IriBTDynReconf<Config>::get_double(Config &config, const std::string &name, template<class Config> void IriBTDynReconf<Config>::init(IriBehaviorTreeFactory &factory) { - BT::PortsList get_string_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::string>("param_value")}; - BT::PortsList get_integer_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<int>("param_value")}; - BT::PortsList get_unsigned_integer_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<unsigned int>("param_value")}; - BT::PortsList get_double_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<double>("param_value")}; - BT::PortsList get_double_vector_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::vector<double>>("param_value")}; - BT::PortsList get_bool_ports = {BT::InputPort<Config>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<bool>("param_value")}; + BT::PortsList get_string_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::string>("param_value")}; + BT::PortsList get_integer_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<int>("param_value")}; + BT::PortsList get_unsigned_integer_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<unsigned int>("param_value")}; + BT::PortsList get_double_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<double>("param_value")}; + BT::PortsList get_double_vector_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<std::vector<double>>("param_value")}; + BT::PortsList get_bool_ports = {BT::InputPort<Config *>("dyn_config"),BT::InputPort<std::string>("param_name"),BT::OutputPort<bool>("param_value")}; factory.registerSimpleAction("dyn_reconf_get_string", std::bind(&IriBTDynReconf::get_dyn_reconf_string, this, std::placeholders::_1),get_string_ports); factory.registerSimpleAction("dyn_reconf_get_integer", std::bind(&IriBTDynReconf::get_dyn_reconf_integer, this, std::placeholders::_1),get_integer_ports); @@ -192,7 +192,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_string(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_string"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -201,7 +201,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_string(BT::TreeNode& self) return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); std::string value; if(this->get_string(config_value,name_value,value)) @@ -217,7 +217,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_bool(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_bool"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -226,7 +226,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_bool(BT::TreeNode& self) return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); bool value; if(this->get_bool(config_value,name_value,value)) @@ -242,7 +242,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_integer(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_integer"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -251,7 +251,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_integer(BT::TreeNode& self return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); int value; if(this->get_integer(config_value,name_value,value)) @@ -267,7 +267,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_unsigned_integer(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_unsigned_integer"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -276,7 +276,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_unsigned_integer(BT::TreeN return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); unsigned int value; if(this->get_unsigned_integer(config_value,name_value,value)) @@ -292,7 +292,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_double"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -301,7 +301,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double(BT::TreeNode& self) return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); double value; if(this->get_double(config_value,name_value,value)) @@ -317,7 +317,7 @@ template<class Config> BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double_vector(BT::TreeNode& self) { ROS_DEBUG("IriBTDynReconf::get_dyn_reconf_double_vector"); - BT::Optional<Config> config = self.getInput<Config>("dyn_config"); + BT::Optional<Config *> config = self.getInput<Config *>("dyn_config"); BT::Optional<std::string> name = self.getInput<std::string>("param_name"); if ((!config) || (!name)) @@ -326,7 +326,7 @@ BT::NodeStatus IriBTDynReconf<Config>::get_dyn_reconf_double_vector(BT::TreeNode return BT::NodeStatus::FAILURE; } - Config config_value=config.value(); + Config *config_value=config.value(); std::string name_value = name.value(); std::stringstream text; unsigned char dummy;