From a02a37f41efb68de046f13cc77c13385c490b9e3 Mon Sep 17 00:00:00 2001
From: Joan Perez Ibarz <jperez@iri.upc.edu>
Date: Fri, 21 Jan 2011 14:18:20 +0000
Subject: [PATCH] updating algorithm base and fixing template driver
---
.../iri_base_algorithm/iri_base_algorithm.h | 102 ++++++++++++++++++
1 file changed, 102 insertions(+)
diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h
index e69de29..a10569d 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
--
GitLab