From 28174716058e0e926ad4a5b09a45d11366603e74 Mon Sep 17 00:00:00 2001 From: Sergi Hernandez Juan <shernand@iri.upc.edu> Date: Wed, 10 May 2023 07:55:48 +0200 Subject: [PATCH] Added a run and reset parameters to handle the operation of the tree. --- .../iri_base_bt_client/iri_base_bt_client.h | 66 +++++++++++++++++-- src/iri_base_bt_client/submodule.py | 2 + 2 files changed, 62 insertions(+), 6 deletions(-) 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 d626e76..6c217eb 100644 --- a/include/iri_base_bt_client/iri_base_bt_client.h +++ b/include/iri_base_bt_client/iri_base_bt_client.h @@ -113,6 +113,17 @@ class IriBaseBTClient */ ros::Rate loop_rate_; + /** + * \brief + * + */ + bool run; + + /** + * \brief + * + */ + bool reset; protected: /** * \brief @@ -384,6 +395,8 @@ class IriBaseBTClient * */ virtual void user_loop(void) = 0; + + BT::NodeStatus get_config(BT::TreeNode& self); }; @@ -411,11 +424,15 @@ IriBaseBTClient<ConfigClass>::IriBaseBTClient(const ros::NodeHandle &nh) : this->logger_minitrace = NULL; this->logger_file = NULL; this->publisher_zmq = NULL; + this->run=false; + this->reset=false; this->status=BT::NodeStatus::IDLE; // initialize core actions and conditions this->core.init(this->factory); + 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)) ROS_ERROR("IriBaseBTClient: need to define path parameter"); @@ -618,6 +635,7 @@ template <class ConfigClass> void IriBaseBTClient<ConfigClass>::reconfigureCallback(ConfigClass &config, uint32_t level) { std::vector<typename ConfigClass::AbstractParamDescriptionConstPtr> params; + std::vector<typename ConfigClass::AbstractGroupDescriptionConstPtr> groups; boost::any value; double new_rate; bool new_enable; @@ -630,7 +648,22 @@ void IriBaseBTClient<ConfigClass>::reconfigureCallback(ConfigClass &config, uint for(typename std::vector<typename ConfigClass::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++) { (*param)->getValue(config,value); - if((*param)->name=="bt_client_rate") + if((*param)->name=="bt_client_run") + this->run=boost::any_cast<bool &>(value); + else if((*param)->name=="bt_client_reset") + { + this->reset=boost::any_cast<bool &>(value); + groups=ConfigClass::__getGroupDescriptions__(); + for(typename std::vector<typename ConfigClass::AbstractGroupDescriptionConstPtr>::iterator group=groups.begin();group!=groups.end();group++) + { + if((*group)->name=="bt_client_reset") + { + value=false; + (*group)->updateParams(value,config); + } + } + } + else if((*param)->name=="bt_client_rate") { new_rate=boost::any_cast<double &>(value); if(new_rate!=this->getRate()) @@ -699,11 +732,22 @@ void *IriBaseBTClient<ConfigClass>::mainThread(void *param) { try { - iriNode->user_loop(); - iriNode->status=iriNode->tree.tickRoot(); - if(iriNode->status==BT::NodeStatus::FAILURE) - iriNode->tree.haltTree(); - ROS_DEBUG_STREAM("Tree status: " << iriNode->status); + if(iriNode->run) + { + if(iriNode->reset) + { + iriNode->tree.haltTree(); + iriNode->reset=false; + } + else + { + iriNode->user_loop(); + iriNode->status=iriNode->tree.tickRoot(); + if(iriNode->status==BT::NodeStatus::FAILURE) + iriNode->tree.haltTree(); + ROS_DEBUG_STREAM("Tree status: " << iriNode->status); + } + } } catch(BT::LogicError &e) { @@ -766,6 +810,16 @@ int IriBaseBTClient<ConfigClass>::spin(void) return 0; } +template <class ConfigClass> +BT::NodeStatus IriBaseBTClient<ConfigClass>::get_config(BT::TreeNode& self) +{ + ROS_DEBUG("IriBaseBTClient::get_config"); + + self.setOutput("dyn_config", this->config_); + return BT::NodeStatus::SUCCESS; +} + + // definition of the static Ctrl+C counter /** diff --git a/src/iri_base_bt_client/submodule.py b/src/iri_base_bt_client/submodule.py index fc4e7c2..e8a41d5 100644 --- a/src/iri_base_bt_client/submodule.py +++ b/src/iri_base_bt_client/submodule.py @@ -4,6 +4,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * def add_bt_client_params(gen): new_group = gen.add_group('bt_client') + new_group.add("bt_client_run",bool_t,0,"Allow tree execution", False) + new_group.add("bt_client_reset",bool_t,0,"Reset tree execution", False) new_group.add("bt_client_rate",double_t,0,"Desired rate in Hz of the client", 1.0, 0.1, 1000.0) new_group.add("bt_client_enable_cout_logger",bool_t,0,"Enables status printing in terminal", False) new_group.add("bt_client_enable_minitrace_logger",bool_t,0,"Enables duration saving in log file", False) -- GitLab