diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h index 5c0db1c8ea0d746e01ad03cfd922715361d990bd..28986da9c7402d12f79248c826e91dea7a07dba2 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -282,6 +282,8 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : diagnostic_(), dsrv_(private_node_handle_) { + ROS_DEBUG("IRI Base Algorithm Constructor"); + // create thread server instance this->thread_server_ = CThreadServer::instance(); @@ -299,13 +301,14 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() : template <class Algorithm> IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() { + ROS_DEBUG("IRI Base Algorithm Destructor"); + this->thread_server_->kill_thread(this->main_thread_id_); } template <class Algorithm> void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level) { -// this->alg_.config_ = config; this->alg_.config_update(config, level); } @@ -318,6 +321,8 @@ void IriBaseAlgorithm<Algorithm>::addDiagnostics(void) template <class Algorithm> void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) { + ROS_DEBUG("IRI Base Algorithm mainThread"); + // retrieve base algorithm class IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param; @@ -337,6 +342,8 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) template <class Algorithm> int IriBaseAlgorithm<Algorithm>::spin(void) { + ROS_DEBUG("IRI Base Algorithm spin"); + // initialize diagnostics this->diagnostic_.setHardwareID("none"); this->addDiagnostics(); @@ -383,6 +390,8 @@ int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0; template <class AlgImplTempl> int main(int argc, char **argv, std::string node_name) { + ROS_DEBUG("IRI Base Algorithm %s Launched", node_name.c_str()); + // ROS initialization ros::init(argc, argv, node_name);