diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4a1a88953179584a8195a6f9870c351321e2639a..070e7c2c49ced2e730557189bf63ffd390be9711 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -88,7 +88,7 @@ include_directories(${iriutils_INCLUDE_DIR})
## Declare a cpp executable
add_executable(${PROJECT_NAME} src/blob_detector_alg.cpp src/blob_detector_alg_node.cpp)
-add_library(${PROJECT_NAME}_nodelet src/blob_detector_alg.cpp src/blob_detector_alg_nodelet.cpp)
+add_library(${PROJECT_NAME}_nodelet src/blob_detector_alg.cpp src/blob_detector_alg_node.cpp src/blob_detector_alg_nodelet.cpp)
# ********************************************************************
# Add the libraries
diff --git a/include/blob_detector_alg_node.h b/include/blob_detector_alg_node.h
index a42a1a9d0bd69b28129653074e52c89e52723cca..e0dde833c8592d5484f415df0468ba1fbbe5c9a9 100644
--- a/include/blob_detector_alg_node.h
+++ b/include/blob_detector_alg_node.h
@@ -94,7 +94,7 @@ class BlobDetectorAlgNode : public algorithm_base::IriBaseAlgorithm<BlobDetector
* This constructor initializes specific class attributes and all ROS
* communications variables to enable message exchange.
*/
- BlobDetectorAlgNode(void);
+ BlobDetectorAlgNode(const ros::NodeHandle &nh = ros::NodeHandle("~"));
/**
* \brief Destructor
diff --git a/include/blob_detector_alg_nodelet.h b/include/blob_detector_alg_nodelet.h
index 4d264c224e9520003b8c6defa42a621c9e1e9d9f..2646b862c8a347ec4ebec52a1b3fb5f244369ffd 100644
--- a/include/blob_detector_alg_nodelet.h
+++ b/include/blob_detector_alg_nodelet.h
@@ -26,116 +26,15 @@
#define _blob_detector_alg_nodelet_h_
#include "nodelet/nodelet.h"
+#include "blob_detector_alg_node.h"
-#include <iri_base_algorithm/iri_base_algorithm.h>
-#include "blob_detector_alg.h"
-#include "cv_bridge/cv_bridge.h"
-
-// [publisher subscriber headers]
-#include <sensor_msgs/Image.h>
-#include <image_transport/image_transport.h>
-
-// [service client headers]
-
-// [action server client headers]
-
-#include "eventserver.h"
#include "threadserver.h"
-/**
- * \brief IRI ROS Specific Algorithm Class
- *
- */
-class BlobDetectorAlgNodelet : public algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>
-{
- private:
- // [publisher attributes]
- image_transport::CameraPublisher image_out_publisher_;
- sensor_msgs::ImagePtr image_out_Image_msg_;
-
- // [subscriber attributes]
- image_transport::ImageTransport it;
- image_transport::CameraSubscriber image_in_subscriber_;
- void image_in_callback(const sensor_msgs::Image::ConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& info);
- pthread_mutex_t image_in_mutex_;
- void image_in_mutex_enter(void);
- void image_in_mutex_exit(void);
-
- // [service attributes]
-
- // [client attributes]
-
- // [action server attributes]
-
- // [action client attributes]
-
- cv_bridge::CvImageConstPtr image_;
- cv_bridge::CvImagePtr image_out_;
-
- public:
- /**
- * \brief Constructor
- *
- * This constructor initializes specific class attributes and all ROS
- * communications variables to enable message exchange.
- */
- BlobDetectorAlgNodelet(ros::NodeHandle private_nh);
-
- /**
- * \brief Destructor
- *
- * This destructor frees all necessary dynamic memory allocated within this
- * this class.
- */
- ~BlobDetectorAlgNodelet(void);
-
- protected:
- /**
- * \brief main node thread
- *
- * This is the main thread node function. Code written here will be executed
- * in every node loop while the algorithm is on running state. Loop frequency
- * can be tuned by modifying loop_rate attribute.
- *
- * Here data related to the process loop or to ROS topics (mainly data structs
- * related to the MSG and SRV files) must be updated. ROS publisher objects
- * must publish their data in this process. ROS client servers may also
- * request data to the corresponding server topics.
- */
- void mainNodeThread(void);
-
- /**
- * \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.
- */
- void node_config_update(Config &config, uint32_t level);
-
- /**
- * \brief node add diagnostics
- *
- * In this abstract function additional ROS diagnostics applied to the
- * specific algorithms may be added.
- */
- void addNodeDiagnostics(void);
-
- // [diagnostic functions]
-
- // [test functions]
-};
-
class BlobDetectorNodelet : public nodelet::Nodelet
{
private:
virtual void onInit();// initialization function
- BlobDetectorAlgNodelet *dev;
+ BlobDetectorAlgNode *dev;
// thread attributes
CThreadServer *thread_server;
std::string spin_thread_id;
diff --git a/src/blob_detector_alg_node.cpp b/src/blob_detector_alg_node.cpp
index 0bca8fab7c3b185bf54027e7eb927afa301908c5..ebddc0646d93b8f4b7a53183efa258a16ec63c58 100644
--- a/src/blob_detector_alg_node.cpp
+++ b/src/blob_detector_alg_node.cpp
@@ -1,7 +1,7 @@
#include "blob_detector_alg_node.h"
-BlobDetectorAlgNode::BlobDetectorAlgNode(void) :
- algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(),it(this->public_node_handle_)
+BlobDetectorAlgNode::BlobDetectorAlgNode(const ros::NodeHandle &nh) :
+ algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(nh),it(nh)
{
//init class attributes if necessary
this->loop_rate_ = 1;//in [Hz]
diff --git a/src/blob_detector_alg_nodelet.cpp b/src/blob_detector_alg_nodelet.cpp
index 250d6c0688b161a82242e2ea42d2da602d33e66b..e4220b04df8882588b487263edcc57f50f3d9e78 100644
--- a/src/blob_detector_alg_nodelet.cpp
+++ b/src/blob_detector_alg_nodelet.cpp
@@ -1,96 +1,3 @@
-#include "blob_detector_alg_nodelet.h"
-
-BlobDetectorAlgNodelet::BlobDetectorAlgNodelet(ros::NodeHandle private_nh) :
- algorithm_base::IriBaseAlgorithm<BlobDetectorAlgorithm>(),it(private_nh)
-{
- //init class attributes if necessary
- this->loop_rate_ = 1;//in [Hz]
-
- // [init publishers]
- this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
-
- // [init subscribers]
- this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 1, &BlobDetectorAlgNodelet::image_in_callback,this);
- pthread_mutex_init(&this->image_in_mutex_,NULL);
-
- this->image_out_=cv_bridge::CvImagePtr(new cv_bridge::CvImage);
- this->image_=cv_bridge::CvImagePtr(new cv_bridge::CvImage);
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
-}
-
-BlobDetectorAlgNodelet::~BlobDetectorAlgNodelet(void)
-{
- // [free dynamic memory]
- pthread_mutex_destroy(&this->image_in_mutex_);
-}
-
-void BlobDetectorAlgNodelet::mainNodeThread(void)
-{
- // [fill msg structures]
- //this->image_out_Image_msg_.data = my_var;
-
- // [fill srv structure and make request to the server]
-
- // [fill action structure and make request to the action server]
-
- // [publish messages]
-}
-
-/* [subscriber callbacks] */
-void BlobDetectorAlgNodelet::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info)
-{
- try{
- this->image_ = cv_bridge::toCvShare(msg, "rgb8");
- this->image_out_->encoding=this->image_->encoding;
- this->image_out_->header=this->image_->header;
-// this->alg_.blob_detector(this->image_->image,this->image_out_->image);
- this->image_out_publisher_.publish(this->image_out_->toImageMsg(),info);
- }catch(cv_bridge::Exception& e){
- ROS_ERROR("cv_bridge exception: %s", e.what());
- }
-
- //use appropiate mutex to shared variables if necessary
- //this->alg_.lock();
- //this->image_in_mutex_enter();
-
- //std::cout << msg->data << std::endl;
-
- //unlock previously blocked shared variables
- //this->alg_.unlock();
- //this->image_in_mutex_exit();
-}
-
-void BlobDetectorAlgNodelet::image_in_mutex_enter(void)
-{
- pthread_mutex_lock(&this->image_in_mutex_);
-}
-
-void BlobDetectorAlgNodelet::image_in_mutex_exit(void)
-{
- pthread_mutex_unlock(&this->image_in_mutex_);
-}
-
-/* [service callbacks] */
-
-/* [action callbacks] */
-
-/* [action requests] */
-
-void BlobDetectorAlgNodelet::node_config_update(Config &config, uint32_t level)
-{
-}
-
-void BlobDetectorAlgNodelet::addNodeDiagnostics(void)
-{
-}
-
#include "blob_detector_alg_nodelet.h"
#include <pluginlib/class_list_macros.h>
@@ -101,10 +8,11 @@ BlobDetectorNodelet::BlobDetectorNodelet()
void BlobDetectorNodelet::onInit()
{
- this->dev=new BlobDetectorAlgNodelet(getPrivateNodeHandle());
+ this->dev=new BlobDetectorAlgNode(this->getPrivateNodeHandle());
// initialize the thread
+ std::cout << this->getName() << "," << this->getPrivateNodeHandle().getNamespace() << std::endl;
this->thread_server=CThreadServer::instance();
- this->spin_thread_id=getName() + "_image_local_bin_nodelet_spin";
+ this->spin_thread_id=getName() + "_blob_detector_nodelet_spin";
std::cout << this->spin_thread_id << std::endl;
this->thread_server->create_thread(this->spin_thread_id);
this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);