diff --git a/CMakeLists.txt b/CMakeLists.txt
index bb182e3eb53c9eaff6afaefd1f90de3214b77d23..87a962234db36cd8e1dbb97023ad839411de8c07 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,9 +19,9 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
# edit the following line to add all the source code files of the library
-# SET(sources ./src/iri_base_driver.cpp)
+# SET(sources ./src/iri_base_algorithm.cpp)
# edit the following line to add all the header files of the library
-# SET(headers ./include/iri_base_algorithm/iri_base_algorithm.h)
+# SET(headers ./include/iri_base_algorithm.h)
# FIND_PACKAGE(iriutils REQUIRED)
@@ -33,5 +33,5 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
-# target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+#target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
diff --git a/include/iri_base_algorithm/iri_base_algorithm.h b/include/iri_base_algorithm/iri_base_algorithm.h
index c1ac22ab8c3963a31a22a7769ba5f9630b45d40d..cb00d8fccff476a2167b8f24bd181eddef456787 100644
--- a/include/iri_base_algorithm/iri_base_algorithm.h
+++ b/include/iri_base_algorithm/iri_base_algorithm.h
@@ -1,3 +1,21 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author Joan Perez
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
#ifndef _IRI_BASE_ALGORITHM_H
#define _IRI_BASE_ALGORITHM_H
@@ -7,6 +25,7 @@
// iri-utils thread server
#include "threadserver.h"
#include "exceptions.h"
+#include "mutex.h"
// boost thread includes for ROS::spin thread
#include <boost/thread.hpp>
@@ -27,17 +46,18 @@ namespace algorithm_base
*/
class AbstractAlgorithmNode
{
- static int ctrl_c_hit_count_;
-
- static void hupCalled(int sig)
- {
- ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
- }
-
- static void sigCalled(int sig)
- {
- ctrl_c_hit_count_++;
- }
+ protected:
+ static int ctrl_c_hit_count_;
+
+ static void hupCalled(int sig)
+ {
+ ROS_WARN("Unexpected SIGHUP caught. Ignoring it.");
+ }
+
+ static void sigCalled(int sig)
+ {
+ ctrl_c_hit_count_++;
+ }
};
/**
@@ -206,8 +226,22 @@ class IriBaseAlgorithm : public AbstractAlgorithmNode
* \param level integer referring the level in which the configuration
* has been changed.
*/
- virtual void reconfigureCallback(Config &config, uint32_t level) = 0;
+ void reconfigureCallback(Config &config, uint32_t level);
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ virtual void node_config_update(Config &config, uint32_t level) = 0;
+
/**
* \brief add diagnostics
*
@@ -261,45 +295,53 @@ IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm() :
public_node_handle_(ros::this_node::getName()),
private_node_handle_("~"),
loop_rate_(DEFAULT_RATE),
- dsrv_(private_node_handle_),
- diagnostic_()
+ diagnostic_(),
+ dsrv_(private_node_handle_)
{
+ ROS_DEBUG("IriBaseAlgorithm::Constructor");
// create thread server instance
this->thread_server_ = CThreadServer::instance();
-
+
// identify main thread
this->main_thread_id_ = "main_thread";
// create main thread and attach to server
this->thread_server_->create_thread(this->main_thread_id_);
this->thread_server_->attach_thread(this->main_thread_id_, this->mainThread, this);
-
- // assign callback to dynamic reconfigure server
- dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
+// // assign callback to dynamic reconfigure server
+// dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
}
template <class Algorithm>
IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm()
{
- std::cout << "I'm leaving for good" << std::endl << std::endl;
+ ROS_DEBUG("IriBaseAlgorithm::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;
-// }
+template <class Algorithm>
+void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level)
+{
+ ROS_DEBUG("IriBaseAlgorithm::reconfigureCallback");
+ this->node_config_update(config, level);
+ this->alg_.config_update(config, level);
+}
template <class Algorithm>
void IriBaseAlgorithm<Algorithm>::addDiagnostics(void)
{
+ ROS_DEBUG("IriBaseAlgorithm::addDiagnostics");
addNodeDiagnostics();
}
template <class Algorithm>
void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
{
+ ROS_DEBUG("IriBaseAlgorithm::mainThread");
+
// retrieve base algorithm class
IriBaseAlgorithm *iriNode = (IriBaseAlgorithm *)param;
@@ -319,6 +361,8 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param)
template <class Algorithm>
int IriBaseAlgorithm<Algorithm>::spin(void)
{
+ ROS_DEBUG("IriBaseAlgorithm::spin");
+
// initialize diagnostics
this->diagnostic_.setHardwareID("none");
this->addDiagnostics();
@@ -326,7 +370,10 @@ int IriBaseAlgorithm<Algorithm>::spin(void)
// launch ros spin in different thread
this->ros_thread_.reset( new boost::thread(boost::bind(&ros::spin)) );
assert(ros_thread_);
-
+
+ // assign callback to dynamic reconfigure server
+ this->dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2));
+
// launch node thread
this->thread_server_->start_thread( this->main_thread_id_);
@@ -334,6 +381,8 @@ int IriBaseAlgorithm<Algorithm>::spin(void)
{
// update diagnostics
this->diagnostic_.update();
+
+ ros::WallDuration(0.1).sleep();
}
// stop ros
@@ -352,7 +401,7 @@ int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
* \brief base main
*
* This main is common for all the algorithm nodes. The AlgImplTempl class
- * refers to an specific implementation of a generic algorithm.
+ * refers to an specific implementation derived from a generic algorithm.
*
* First, ros::init is called providing the node name. Ctrl+C control is
* activated for sercure and safe exit. Finally, an AlgImplTempl object is
@@ -365,9 +414,11 @@ int AbstractAlgorithmNode::ctrl_c_hit_count_ = 0;
template <class AlgImplTempl>
int main(int argc, char **argv, std::string node_name)
{
+ ROS_DEBUG("IriBaseAlgorithm::%s Launched", node_name.c_str());
+
// ROS initialization
ros::init(argc, argv, node_name);
-
+
// allow Ctrl+C management
signal(SIGHUP, &AbstractAlgorithmNode::hupCalled);