diff --git a/CMakeLists.txt b/CMakeLists.txt index f8f1c9ccad793e5385c2eb83ffd7ed7b15c5b343..bb182e3eb53c9eaff6afaefd1f90de3214b77d23 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.4.6) include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +set(PROJECT_NAME iri_base_algorithm) + # Set the build type. Options are: # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage # Debug : w/ debug symbols, w/o optimization @@ -16,15 +18,20 @@ set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #set the default path for built libraries to the "lib" directory set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) -#uncomment if you have defined messages -#rosbuild_genmsg() -#uncomment if you have defined services -#rosbuild_gensrv() +# edit the following line to add all the source code files of the library +# SET(sources ./src/iri_base_driver.cpp) +# edit the following line to add all the header files of the library +# SET(headers ./include/iri_base_algorithm/iri_base_algorithm.h) + +# FIND_PACKAGE(iriutils REQUIRED) + +# INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR} ${headers}) #common commands for building c++ executables and libraries -#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +# rosbuild_add_library(${PROJECT_NAME} SHARED ${sources}) #target_link_libraries(${PROJECT_NAME} another_library) #rosbuild_add_boost_directories() #rosbuild_link_boost(${PROJECT_NAME} thread) #rosbuild_add_executable(example examples/example.cpp) -#target_link_libraries(example ${PROJECT_NAME}) +# 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 0ca9e6295f8f2cd3233655767f3ff45e5a6035c1..e69b4291e1d811f1bb163938b3da6fe5aec2c0ad 100644 --- a/include/iri_base_algorithm/iri_base_algorithm.h +++ b/include/iri_base_algorithm/iri_base_algorithm.h @@ -2,11 +2,14 @@ #define _IRI_BASE_ALGORITHM_H #include <ros/ros.h> +// #include <boost/bind.hpp> +#include <dynamic_reconfigure/server.h> // iri-utils thread server #include "threadserver.h" #include "exceptions.h" +//check me out // namespace algorithm_base // { @@ -17,15 +20,23 @@ template <class Algorithm> class IriBaseAlgorithm { + public: + //check me out + typedef typename Algorithm::Config Config; + protected: - Algorithm alg; - CThreadServer *thread_server; - std::string main_thread_id; + Algorithm alg_; + CThreadServer *thread_server_; + std::string main_thread_id_; ros::NodeHandle node_handle_; ros::NodeHandle private_node_handle_; - ros::Rate loop_rate; + //check me out + dynamic_reconfigure::Server<Config> dsrv_; +// dynamic_reconfigure::Server<Config>::CallbackType cb; + + ros::Rate loop_rate_; static const unsigned int DEFAULT_RATE = 10; //[Hz] public: @@ -33,11 +44,12 @@ class IriBaseAlgorithm ~IriBaseAlgorithm(); protected: -// void addDiagnostics(void); + void reconfigureCallback(Config &config, uint32_t level); + void addDiagnostics(void); // void addOpenedTests(void); // void addStoppedTests(void); // void addRunningTests(void); -// virtual void addNodeDiagnostics(void) = 0; + virtual void addNodeDiagnostics(void) = 0; // virtual void addNodeOpenedTests(void) = 0; // virtual void addNodeStoppedTests(void) = 0; // virtual void addNodeRunningTests(void) = 0; @@ -46,20 +58,45 @@ class IriBaseAlgorithm }; +//check me out template <class Algorithm> -IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), loop_rate(DEFAULT_RATE) +IriBaseAlgorithm<Algorithm>::IriBaseAlgorithm(): private_node_handle_("~"), dsrv_(ros::NodeHandle("~")), loop_rate_(DEFAULT_RATE) { // create the status thread - this->thread_server=CThreadServer::instance(); - this->main_thread_id="main_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); + 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_); + //check me out +// dynamic_reconfigure::Server<Config>::CallbackType cb; + // = boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2); + dsrv_.setCallback(boost::bind(&IriBaseAlgorithm<Algorithm>::reconfigureCallback, this, _1, _2)); + // initialize class attributes } +template <class Algorithm> +IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() +{ + this->thread_server_->kill_thread(this->main_thread_id_); +} + +template <class Algorithm> +void IriBaseAlgorithm<Algorithm>::reconfigureCallback(Config &config, uint32_t level) +{ + //check me out + alg_.config_ = config; +} + +template <class Algorithm> +void IriBaseAlgorithm<Algorithm>::addDiagnostics(void) +{ + addNodeDiagnostics(); +} + template <class Algorithm> void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) { @@ -68,27 +105,28 @@ void *IriBaseAlgorithm<Algorithm>::mainThread(void *param) while(ros::ok()) { iriNode->mainNodeThread(); - iriNode->loop_rate.sleep(); + iriNode->loop_rate_.sleep(); } pthread_exit(NULL); } -template <class Algorithm> -IriBaseAlgorithm<Algorithm>::~IriBaseAlgorithm() +namespace algorithm_base { - 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); + //check me out AlgImplTempl algImpl; - return ros::spin(); + //check me out + ros::spin(); + + return 0; } -// } +} #endif diff --git a/manifest.xml b/manifest.xml index 906e3923e29caa580fe711a281e5fddd3da44815..77c951eb8f697bf92e418f6e5a1e2f4987815f68 100644 --- a/manifest.xml +++ b/manifest.xml @@ -13,8 +13,7 @@ <depend package="dynamic_reconfigure"/> <export> - <cpp cflags="-I${prefix}/include" - lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_base_algorithm"/> + <cpp cflags="-I${prefix}/include"/> </export> </package>