From d6228910091b3c6378b0d6a6101690e3a7db30c8 Mon Sep 17 00:00:00 2001
From: Joan Perez Ibarz <jperez@iri.upc.edu>
Date: Thu, 3 Feb 2011 15:31:54 +0000
Subject: [PATCH] Adding iri_common_msgs package into the iri_common_drivers
stack
Updating iri_base_driver
---
.../iri_base_algorithm/iri_base_algorithm.h | 181 +++++++++++++-----
1 file changed, 132 insertions(+), 49 deletions(-)
diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h
index 5c4a17b..e53d146 100644
--- a/include/iri_base_algorithm/iri_base_algorithm.h
+++ b/include/iri_base_algorithm/iri_base_algorithm.h
@@ -8,97 +8,186 @@
#include "threadserver.h"
#include "exceptions.h"
-//check me out
-#define INTERNAL_SPIN
-#define DYNAMIC_CFG
-#define DIAGNOSTICS
+// boost thread includes for ROS::spin thread
+#include <boost/thread.hpp>
+#include <boost/bind.hpp>
-#ifdef INTERNAL_SPIN
- #include <boost/thread.hpp>
- #include <boost/bind.hpp>
-#endif
-
-#ifdef DYNAMIC_CFG
+// dynamic reconfigure server include
#include <dynamic_reconfigure/server.h>
-#endif
-#ifdef DIAGNOSTICS
+// diagnostic updater include
#include <diagnostic_updater/diagnostic_updater.h>
-#endif
-
-// //check me out
-// namespace algorithm_base
-// {
//check me out
+#define INTERNAL_SPIN
+#define DYNAMIC_CFG
+#define DIAGNOSTICS
+
+namespace algorithm_base
+{
+
+/**
+ * \brief Abstract Class for Ctrl+C management
+ *
+ */
class AbstractAlgorithmNode
{
static int ctrl_c_hit_count_;
static void hupCalled(int sig)
{
- ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
+ ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
}
static void sigCalled(int sig)
{
- ctrl_c_hit_count_++;
+ ctrl_c_hit_count_++;
}
};
+/**
+ * \brief IRI ROS Algorithm Base Node Class
+ *
+ * This class provides a common framework for all kind of algorithms, similar to
+ * the one defined for the ROS driver_base::DriverNode<TemplateDriver> for the
+ * driver environment. In this case, the template Algorithm class must
+ * In addition, a mainThread is provided like in the IriDriver
+ */
+
/**
* \brief IRI ROS Base Node Class
*
+ * This class inherits from the ROS class driver_base::DriverNode<TemplateDriver>
+ * to provide an execution thread to the driver object. The TemplateDriver object
+ * must be an implementation of the CIriDriver class. The inherit template design
+ * form allows complete access to the driver object while manteining flexibility
+ * to instantiate any object inherit from CIriDriver.
+ *
+ * A shared library is generated together with CIriDriver class to force
+ * implementation of virtual methods to final user. Functions from ROS driver
+ * base class to perform tests, add diagnostics or hooks, are mainteined to
+ * implement common purpose behaviours and forwarded to the final user node class
+ * for specification.
+ *
+ * Threads are implemented using iri_utils software utilities. The mainThread()
+ * function loops in a defined loop_rate_. In each iteration, the abstract
+ * mainNodeThread() function defined in the inherit node class is called.
+ *
+ * Similarly, functions such as addDiagnostics() or addRunningTests() are prepared
+ * for detailing common features for all driver nodes while executing specific
+ * commands for each specification by calling the respective functions
+ * addNodeDiagnostics() and addNodeRunningTests() from the inherit node
+ * implementation.
+ *
+ * Instances of both CIriDriver and IriBaseNodeDriver can be easly generated with the
+ * iri_ros_scripts package. Similarly, data can be sent and read through ROS
+ * topics by using those scripts. The scripts can be downloaded from the
+ * iri_stack SVN.
*/
template <class Algorithm>
class IriBaseAlgorithm : public AbstractAlgorithmNode
{
-#ifdef DYNAMIC_CFG
public:
- //check me out
+ /**
+ * \brief config object
+ *
+ * All template Algorithm 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 typename Algorithm::Config Config;
-#endif
protected:
- Algorithm alg_;
+ /**
+ * \brief Reference to the unique thread handler
+ *
+ * This reference to the unique thread handler is initialized when an object
+ * of this class is first created. It is used to create and handle all the
+ * IriBaseNodeDriver threads. The object pointed by this reference is shared
+ * by all objects in any application.
+ */
CThreadServer *thread_server_;
+
+ /**
+ * \brief identifier of the node main thread
+ *
+ * This string has the identifier of the main IriBaseNodeDriver thread.
+ * This string is initialized at contruction time. This thread is only used
+ * internally to the class, so it is not possible to get its identifier out.
+ */
std::string main_thread_id_;
- ros::NodeHandle node_handle_;
- ros::NodeHandle private_node_handle_;
+ /**
+ * \brief template algorithm class
+ *
+ * This template class refers to an implementation of an specific algorithm
+ * interface. Will be used in the derivate class to define the common
+ * behaviour for all the different implementations from the same algorithm.
+ */
+ Algorithm alg_;
- //check me out
- #ifdef DYNAMIC_CFG
- dynamic_reconfigure::Server<Config> dsrv_;
-// dynamic_reconfigure::Server<Config>::CallbackType cb;
- #endif
+ /**
+ * \brief node handle communication object
+ *
+ * This node handle is going to be used to create topics and services within
+ * the node namespace. Additional node handles can be instantatied if
+ * additional namespaces are needed.
+ */
+ ros::NodeHandle node_handle_;
- #ifdef DIAGNOSTICS
- diagnostic_updater::Updater diagnostic_;
- #endif
+ /**
+ * \brief private node handle object
+ *
+ * This private node handle will be used to define algorithm parameters into
+ * the ROS parametre server. For communication pruposes please use the
+ * previously defined node_handle_ object.
+ */
+ ros::NodeHandle private_node_handle_;
+ /**
+ * \brief main thread loop rate
+ *
+ * This value determines the loop frequency of the node main thread function
+ * mainThread() in HZ. It is initialized at construction time. This variable
+ * may be modified in the node implementation constructor if a desired
+ * frequency is required.
+ */
ros::Rate loop_rate_;
+
+ /**
+ * \brief default main thread frequency
+ *
+ * This constant determines the default frequency of the mainThread() in HZ.
+ * All nodes will loop at this rate if loop_rate_ variable is not modified.
+ */
static const unsigned int DEFAULT_RATE = 10; //[Hz]
+
+ /**
+ * \brief diagnostic updater
+ *
+ * The diagnostic updater allows definition of custom diagnostics.
+ *
+ */
+ diagnostic_updater::Updater diagnostic_;
public:
IriBaseAlgorithm();
~IriBaseAlgorithm();
-#ifdef INTERNAL_SPIN
int spin(void);
private:
+ //check me out
boost::shared_ptr<boost::thread> ros_thread_;
-#endif
+
+ dynamic_reconfigure::Server<Config> dsrv_;
+// dynamic_reconfigure::Server<Config>::CallbackType cb;
protected:
-#ifdef DYNAMIC_CFG
void reconfigureCallback(Config &config, uint32_t level);
-#endif
-#ifdef DIAGNOSTICS
void addDiagnostics(void);
virtual void addNodeDiagnostics(void) = 0;
-#endif
+
// void addOpenedTests(void);
// void addStoppedTests(void);
// void addRunningTests(void);
@@ -113,12 +202,8 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
template <class Algorithm>
IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() :
private_node_handle_("~"),
-#ifdef DYNAMIC_CFG
- dsrv_(ros::NodeHandle("~")),
-#endif
-#ifdef DIAGNOSTICS
+ dsrv_(private_node_handle_),
diagnostic_(),
-#endif
loop_rate_(DEFAULT_RATE)
{
// create the status thread
@@ -133,12 +218,10 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() :
#endif
//check me out
-#ifdef DYNAMIC_CFG
// dynamic_reconfigure::Server<Config>::CallbackType cb;
// = boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2);
dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
-#endif
- // initialize class attributes
+
}
template <class Algorithm>
@@ -235,8 +318,8 @@ int IriBaseAlgorithm<Algorithm>::spin(void)
int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
-namespace algorithm_base
-{
+// namespace algorithm_base
+// {
template <class AlgImplTempl>
int main(int argc, char **argv, std::string name)
--
GitLab