From 2f29f484725e049b0e5f3b63b56c04b3deea6079 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Thu, 4 Jun 2020 17:24:38 +0200
Subject: [PATCH] Solved a bug with the order in which the attributes of the
class were initialized: the wrong order caused a segmentation fault. Catched
the BT::RunTime exception while trying to import the tree. Partially
implemented the dynamic reconfigure callback.
---
.../iri_base_bt_client/iri_base_bt_client.h | 134 ++++++++++++------
1 file changed, 87 insertions(+), 47 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 a62a6d3..1642421 100644
--- a/include/iri_base_bt_client/iri_base_bt_client.h
+++ b/include/iri_base_bt_client/iri_base_bt_client.h
@@ -37,6 +37,7 @@
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
+#include <boost/any.hpp>
// dynamic reconfigure server include
#include <dynamic_reconfigure/server.h>
@@ -114,31 +115,6 @@ class IriBaseBTClient
*/
BT::NodeStatus status;
- /**
- * \brief config object
- *
- * All template ConfigClass class will need a Config variable to allow ROS
- * dynamic reconfigure. This config class will contain all algorithm
- * parameters which may be modified once the algorithm node is launched.
- */
- typedef ConfigClass Config;
-
- /**
- * \brief ros spin thread
- *
- * This boost thread object will manage the ros::spin function. It is
- * instantiated and launched in the spin class method.
- */
- boost::shared_ptr<boost::thread> ros_thread_;
-
- /**
- * \brief dynamic reconfigure server
- *
- * The dynamic reconfigure server is in charge to retrieve the parameters
- * defined in the config cfg file through the reconfigureCallback.
- */
- dynamic_reconfigure::Server<Config> dsrv_;
-
/**
* \brief main thread loop rate
*
@@ -149,7 +125,6 @@ class IriBaseBTClient
*/
ros::Rate loop_rate_;
-
protected:
/**
* \brief define config type
@@ -203,6 +178,31 @@ class IriBaseBTClient
*/
diagnostic_updater::Updater diagnostic_;
+ /**
+ * \brief ros spin thread
+ *
+ * This boost thread object will manage the ros::spin function. It is
+ * instantiated and launched in the spin class method.
+ */
+ boost::shared_ptr<boost::thread> ros_thread_;
+
+ /**
+ * \brief dynamic reconfigure server
+ *
+ * The dynamic reconfigure server is in charge to retrieve the parameters
+ * defined in the config cfg file through the reconfigureCallback.
+ */
+ dynamic_reconfigure::Server<ConfigClass> dsrv_;
+
+ /**
+ * \brief config object
+ *
+ * All template ConfigClass class will need a Config variable to allow ROS
+ * dynamic reconfigure. This config class will contain all algorithm
+ * parameters which may be modified once the algorithm node is launched.
+ */
+ ConfigClass config_;
+
public:
/**
* \brief constructor
@@ -332,7 +332,7 @@ class IriBaseBTClient
* \param level integer referring the level in which the configuration
* has been changed.
*/
- void reconfigureCallback(Config &config, uint32_t level);
+ void reconfigureCallback(ConfigClass &config, uint32_t level);
/**
* \brief dynamic reconfigure server callback
@@ -346,7 +346,7 @@ class IriBaseBTClient
* \param level integer referring the level in which the configuration
* has been changed.
*/
- virtual void node_config_update(Config &config, uint32_t level) = 0;
+ virtual void node_config_update(ConfigClass &config, uint32_t level) = 0;
/**
* \brief add diagnostics
@@ -397,6 +397,12 @@ IriBaseBTClient<ConfigClass>::IriBaseBTClient(const ros::NodeHandle &nh) :
{
ROS_DEBUG("IriBaseBTClient::Constructor");
+ if(!this->private_node_handle_.getParam("bt_client_rate", this->config_.bt_client_rate))
+ {
+ ROS_WARN("UserBtClientAlgNode::UserBtClientAlgNode: param 'bt_client_rate' not found");
+ }
+ else
+ this->setRate(this->config_.bt_client_rate);
// allow Ctrl+C management
signal(SIGHUP, &IriBaseBTClient<ConfigClass>::hupCalled);
@@ -494,7 +500,7 @@ void IriBaseBTClient<ConfigClass>::enable_minitrace_logger()
}
else
{
- ROS_INFO("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
+ ROS_DEBUG("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
path_ok=true;
}
}
@@ -527,7 +533,7 @@ void IriBaseBTClient<ConfigClass>::enable_file_logger()
}
else
{
- ROS_INFO("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
+ ROS_DEBUG("IriBaseBTClient: Log directory created: %s", logs_path.c_str());
path_ok=true;
}
}
@@ -593,9 +599,44 @@ void IriBaseBTClient<ConfigClass>::disable_zmq_publisher()
}
template <class ConfigClass>
-void IriBaseBTClient<ConfigClass>::reconfigureCallback(Config &config, uint32_t level)
+void IriBaseBTClient<ConfigClass>::reconfigureCallback(ConfigClass &config, uint32_t level)
{
+ std::vector<typename ConfigClass::AbstractParamDescriptionConstPtr> params;
+ boost::any value;
+ double new_rate;
+ bool new_enable;
+
ROS_DEBUG("IriBaseBTClient::reconfigureCallback");
+
+ // process the iri_base_bt_client parameters
+ this->lock();
+ params=ConfigClass::__getParamDescriptions__();
+ 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")
+ {
+ new_rate=boost::any_cast<double &>(value);
+ if(new_rate!=this->getRate())
+ this->setRate(new_rate);
+ }
+ else if((*param)->name=="iri_bt_client_enable_cout_logger")
+ {
+ new_enable=boost::any_cast<bool &>(value);
+ if(new_enable)
+ {
+ if(this->logger_cout==NULL)
+ this->enable_cout_logger();
+ }
+ else
+ {
+ if(this->logger_cout!=NULL)
+ this->disable_cout_logger();
+ }
+ }
+ }
+ this->unlock();
+
this->node_config_update(config, level);
}
@@ -623,29 +664,28 @@ void *IriBaseBTClient<ConfigClass>::mainThread(void *param)
std::string tree_string = iriNode->path + "/" + iriNode->tree_xml_file + ".xml";
iriNode->tree = iriNode->factory.createTreeFromFile(tree_string);
ROS_DEBUG("IriBaseBTClient: tree created from: '%s'", tree_string.c_str());
+ while(ros::ok())
+ {
+ try
+ {
+ iriNode->status=iriNode->tree.root_node->executeTick();
+ ROS_DEBUG_STREAM("Tree status: " << iriNode->status);
+ }
+ catch(BT::LogicError &e)
+ {
+ ROS_ERROR_STREAM("EXCEPTION " << e.what());
+ }
+ // sleep remainder time
+ iriNode->loop_rate_.sleep();
+ }
}
- catch(BT::LogicError &e)
+ catch(BT::RuntimeError &e)
{
ROS_ERROR_STREAM("IriBaseBTClient: Exception: " << e.what());
ROS_ERROR("IriBaseBTClient: couldn't create tree from file. Exiting");
ros::shutdown();
}
- while(ros::ok())
- {
- try
- {
- iriNode->status=iriNode->tree.root_node->executeTick();
- ROS_DEBUG_STREAM("Tree status: " << iriNode->status);
- }
- catch(BT::LogicError &e)
- {
- ROS_ERROR_STREAM("EXCEPTION " << e.what());
- }
- // sleep remainder time
- iriNode->loop_rate_.sleep();
- }
-
// kill main thread
pthread_exit(NULL);
}
--
GitLab