diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..a10569da95c2c82f455c90ac1f0c5ad9da8c26aa 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -0,0 +1,102 @@ +#ifndef _IRI_BASE_ALGORITHM_H +#define _IRI_BASE_ALGORITHM_H + +#include <ros/ros.h> + +// iri-utils thread server +#include "threadserver.h" +#include "exceptions.h" + +namespace algorithm_base +{ + +/** + * \brief IRI ROS Base Node Class + * + */ +template <class Algorithm> +class IriBaseAlgorithm +{ + protected: + Algorithm alg; + CThreadServer *thread_server; + std::string main_thread_id; + + ros::NodeHandle node_handle_; + ros::NodeHandle private_node_handle_; + + ros::Rate loop_rate; + static const unsigned int DEFAULT_RATE = 10; //[Hz] + + public: + IriBaseAlgorithm(); + ~IriBaseAlgorithm(); + + protected: +// void addDiagnostics(void); +// void addOpenedTests(void); +// void addStoppedTests(void); +// void addRunningTests(void); +// virtual void addNodeDiagnostics(void) = 0; +// virtual void addNodeOpenedTests(void) = 0; +// virtual void addNodeStoppedTests(void) = 0; +// virtual void addNodeRunningTests(void) = 0; + static void *mainThread(void *param); + virtual void mainNodeThread(void) = 0; + +}; + +template <class Algorithm> +IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), loop_rate(DEFAULT_RATE) +{ +// this->driver_.setPostOpenHook(boost::bind(&CIriNode::postOpenHook, this)); + + // create the status thread + this->thread_server=CThreadServer::instance(); + this->main_thread_id="main_thread"; + + this->thread_server->create_thread(this->main_thread_id); + this->thread_server->attach_thread(this->main_thread_id, this->mainThread, this); + this->thread_server->start_thread( this->main_thread_id); + + // initialize class attributes +} + +template <class Algorithm> +void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) +{ + IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param; + + while(ros::ok()) + { +// std::cout << __LINE__ << ": driver state=" << iriNode->driver_.getStateName() << std::endl; +// if(iriNode->driver_.isRunning()) +// { + iriNode->mainNodeThread(); + +// ros::spinOnce(); + iriNode->loop_rate.sleep(); +// } + } + + pthread_exit(NULL); +} + +template <class Algorithm> +IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() +{ + this->thread_server->kill_thread(this->main_thread_id); +} + +template <class AlgImplTempl> +int main(int argc, char **argv, std::string name) +{ + ros::init(argc, argv, name); + AlgImplTempl algImpl; +// algImpl.AlgImplTempl::mainThread(); +ros::spin(); + return 0; +} + +} +#endif