diff --git a/CMakeLists.txt b/CMakeLists.txt index ebaa236ebab10793d1193d17c1858d9a1d844b11..a79a376d61ac5bbc8070d072f3e92a1a427d6b68 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ find_package(catkin REQUIRED) # ******************************************************************** # Add catkin additional components here # ******************************************************************** -find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs image_transport cv_bridge) +find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs image_transport cv_bridge nodelet) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs image_tra # ******************************************************************** # find_package(<dependency> REQUIRED) find_package(OpenCV REQUIRED) +find_package(iriutils REQUIRED) # ******************************************************************** # Add topic, service and action definition here @@ -61,7 +62,7 @@ catkin_package( # ******************************************************************** # Add ROS and IRI ROS run time dependencies # ******************************************************************** - CATKIN_DEPENDS iri_base_algorithm sensor_msgs image_transport cv_bridge + CATKIN_DEPENDS iri_base_algorithm sensor_msgs image_transport cv_bridge nodelet # ******************************************************************** # Add system and labrobotica run time dependencies here # ******************************************************************** @@ -78,6 +79,7 @@ catkin_package( include_directories(include) include_directories(${catkin_INCLUDE_DIRS}) include_directories(${OpenCV_INCLUDE_DIRS}) +include_directories(${iriutils_INCLUDE_DIR}) ## Declare a cpp library # add_library(${PROJECT_NAME} <list of source files>) @@ -85,11 +87,17 @@ include_directories(${OpenCV_INCLUDE_DIRS}) ## Declare a cpp executable add_executable(${PROJECT_NAME} src/image_local_binarization_alg.cpp src/image_local_binarization_alg_node.cpp) +add_library(${PROJECT_NAME}_nodelet src/image_local_binarization_alg.cpp src/image_local_binarization_alg_nodelet.cpp) + # ******************************************************************** # Add the libraries # ******************************************************************** target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS}) +target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY}) +target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_nodelet ${OpenCV_LIBS}) +target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY}) # ******************************************************************** # Add message headers dependencies diff --git a/image_local_bin_nodelet_plugin.xml b/image_local_bin_nodelet_plugin.xml new file mode 100644 index 0000000000000000000000000000000000000000..92bf142a04e5714b22be754a5b07420d72cbb405 --- /dev/null +++ b/image_local_bin_nodelet_plugin.xml @@ -0,0 +1,8 @@ +<library path="lib/libiri_image_local_binarization_nodelet"> + <class name="iri_image_local_binarization/ImageLocalBinNodelet" type="ImageLocalBinNodelet" base_class_type="nodelet::Nodelet"> + <description> + Nodelete for the iri firewire camera module + </description> + </class> +</library> + diff --git a/include/image_local_binarization_alg.h b/include/image_local_binarization_alg.h index 467270ceb8bf5df597bcb745ad199744b0d63c37..02600a1e6040e5a6fe5162391c2c6997883de9a9 100644 --- a/include/image_local_binarization_alg.h +++ b/include/image_local_binarization_alg.h @@ -121,7 +121,7 @@ class ImageLocalBinarizationAlgorithm // here define all image_local_binarization_alg interface methods to retrieve and set // the driver parameters - void localThreshold(cv::Mat& img, cv::Mat& dst, double k_, int window); + void localThreshold(const cv::Mat& img, cv::Mat& dst, double k_, int window); /** * \brief Destructor diff --git a/include/image_local_binarization_alg_nodelet.h b/include/image_local_binarization_alg_nodelet.h new file mode 100644 index 0000000000000000000000000000000000000000..4b0f052e878b482bd43f5d36b96ecdb98558ba77 --- /dev/null +++ b/include/image_local_binarization_alg_nodelet.h @@ -0,0 +1,155 @@ +// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. +// Author +// 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/>. +// +// IMPORTANT NOTE: This code has been generated through a script from the +// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness +// of the scripts. ROS topics can be easly add by using those scripts. Please +// refer to the IRI wiki page for more information: +// http://wikiri.upc.es/index.php/Robotics_Lab + +#ifndef _image_local_binarization_alg_nodelet_h_ +#define _image_local_binarization_alg_nodelet_h_ + +#include "nodelet/nodelet.h" + +#include <iri_base_algorithm/iri_base_algorithm.h> +#include "image_local_binarization_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 ImageLocalBinarizationAlgNodelet : public algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm> +{ + private: + // [publisher attributes] + image_transport::CameraPublisher image_out_publisher_; + sensor_msgs::ImagePtr image_out_Image_msg_; + image_transport::Publisher histogram_publisher_; + sensor_msgs::ImagePtr histogram_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] + + int window_size_; + double k_; + + 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. + */ + ImageLocalBinarizationAlgNodelet(ros::NodeHandle private_nh); + + /** + * \brief Destructor + * + * This destructor frees all necessary dynamic memory allocated within this + * this class. + */ + ~ImageLocalBinarizationAlgNodelet(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 ImageLocalBinNodelet : public nodelet::Nodelet +{ + private: + virtual void onInit();// initialization function + ImageLocalBinarizationAlgNodelet *dev; + // thread attributes + CThreadServer *thread_server; + std::string spin_thread_id; + protected: + static void *spin_thread(void *param); + public: + ImageLocalBinNodelet(); + ~ImageLocalBinNodelet(); +}; + + +#endif diff --git a/launch/usb_cam_tag.launch b/launch/usb_cam_tag.launch index 0ddb4b0fbed0c9613bbe2ad088acc6d0e8ff2bea..0393844d763a9f61fb337b426757450016a97ce8 100644 --- a/launch/usb_cam_tag.launch +++ b/launch/usb_cam_tag.launch @@ -9,20 +9,43 @@ <param name="video_device" type="string" value="/dev/video0"/> <param name="camera_frame_id" type="string" value="usb_cam"/> <param name="io_method" type="string" value="mmap"/> - <param name="image_width" type="int" value="640"/> - <param name="image_height" type="int" value="480"/> + <param name="image_width" type="int" value="1920"/> + <param name="image_height" type="int" value="1080"/> <param name="pixel_format" type="string" value="yuyv"/> + <param name="framerate" type="int" value="3"/> <param name="camera_info_url" type="string" value="file://$(find iri_image_local_binarization)/calibration/usb_cam.yaml" /> + <remap from="/usb_cam/camera_info" + to="/camera/camera_info"/> + <remap from="/usb_cam/image_raw" + to="/camera/image_raw"/> </node> + <!-- published topics: /processed_image --> + <!-- subscribed topics: /camera/image_raw --> + <!-- /camera/camera_info --> + <!-- service clients: --> + <!-- service servers: --> + <!-- action clients: --> + <!-- action servers: --> + <!-- image crop --> + <node pkg ="nodelet" + type="nodelet" + name="image_proc_dec" + args="standalone image_proc/crop_decimate"> + <param name="decimation_x" value="2" /> + <param name="decimation_y" value="1" /> + </node> + + <node name="local_th" pkg="iri_image_local_binarization" type="iri_image_local_binarization" output="screen"> + <param name="window_size" value="15"/> <remap from="/local_th/image_in/camera_info" - to="/usb_cam/camera_info"/> + to="/camera_out/camera_info"/> <remap from="/local_th/image_in/image_raw" - to="/usb_cam/image_raw"/> + to="/camera_out/image_raw"/> </node> <node name="image_th" @@ -33,7 +56,7 @@ to="/local_th/image_out/image_raw"/> </node> - <node name="ar_pose" +<!-- <node name="ar_pose" pkg ="ar_pose" type="ar_multi" respawn="false" @@ -45,7 +68,7 @@ to="/local_th/image_out/image_raw"/> <remap from="/camera/camera_info" to="/local_th/image_out/camera_info"/> - </node> + </node>--> <!-- <node name="world2camera" pkg ="tf" diff --git a/package.xml b/package.xml index aa0aeda6835217c106e4c155c698cc3cc9faa7aa..3a7bafc3af0db49f3e3f45e17daaec7262eea3cf 100644 --- a/package.xml +++ b/package.xml @@ -44,11 +44,13 @@ <build_depend>sensor_msgs</build_depend> <build_depend>image_transport</build_depend> <build_depend>OpenCV</build_depend> + <build_depend>nodelet</build_depend> <build_depend>cv_bridge</build_depend> <run_depend>iri_base_algorithm</run_depend> <run_depend>sensor_msgs</run_depend> <run_depend>image_transport</run_depend> <run_depend>OpenCV</run_depend> + <run_depend>nodelet</run_depend> <run_depend>cv_bridge</run_depend> @@ -58,6 +60,7 @@ <!-- <metapackage/> --> <!-- Other tools can request additional information be placed here --> + <nodelet plugin="${prefix}/image_local_bin_nodelet_plugin.xml" /> </export> </package> diff --git a/src/image_local_binarization_alg.cpp b/src/image_local_binarization_alg.cpp index 435f74db4aa2801ace7f0fd3634bbe74ba7fb23c..64eaf7b1fb34d46481b2be85ce7a467ab4ad88fd 100644 --- a/src/image_local_binarization_alg.cpp +++ b/src/image_local_binarization_alg.cpp @@ -28,7 +28,7 @@ void ImageLocalBinarizationAlgorithm::config_update(Config& new_cfg, uint32_t le threshold computation * int window: size of the window for mean computation */ -void ImageLocalBinarizationAlgorithm::localThreshold(cv::Mat& img, cv::Mat& dst, double k_, int window) +void ImageLocalBinarizationAlgorithm::localThreshold(const cv::Mat& img, cv::Mat& dst, double k_, int window) { this->lock(); cv::Mat mean(img.size(), CV_32F); diff --git a/src/image_local_binarization_alg_nodelet.cpp b/src/image_local_binarization_alg_nodelet.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a1e193c3c36292c7b14680a43fbc530bc3fb4568 --- /dev/null +++ b/src/image_local_binarization_alg_nodelet.cpp @@ -0,0 +1,147 @@ +#include "image_local_binarization_alg_nodelet.h" + +ImageLocalBinarizationAlgNodelet::ImageLocalBinarizationAlgNodelet(ros::NodeHandle private_nh) : + algorithm_base::IriBaseAlgorithm<ImageLocalBinarizationAlgorithm>(),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", 10); + this->histogram_publisher_ = this->it.advertise("hystogram", 1); + + // [init subscribers] + this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 10, &ImageLocalBinarizationAlgNodelet::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] + + this->window_size_ = 3; + this->k_ = 0.06; +} + +ImageLocalBinarizationAlgNodelet::~ImageLocalBinarizationAlgNodelet(void) +{ + // [free dynamic memory] + pthread_mutex_destroy(&this->image_in_mutex_); +} + +void ImageLocalBinarizationAlgNodelet::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 ImageLocalBinarizationAlgNodelet::image_in_callback(const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info) +{ + try{ + this->image_ = cv_bridge::toCvShare(msg, "mono8"); + this->image_out_->encoding=this->image_->encoding; + this->image_out_->header=this->image_->header; + this->alg_.localThreshold(this->image_->image, this->image_out_->image, this->k_,(this->window_size_ % 2 == 0 ? this->window_size_ + 1: this->window_size_)); + 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 ImageLocalBinarizationAlgNodelet::image_in_mutex_enter(void) +{ + pthread_mutex_lock(&this->image_in_mutex_); +} + +void ImageLocalBinarizationAlgNodelet::image_in_mutex_exit(void) +{ + pthread_mutex_unlock(&this->image_in_mutex_); +} + +/* [service callbacks] */ + +/* [action callbacks] */ + +/* [action requests] */ + +void ImageLocalBinarizationAlgNodelet::node_config_update(Config &config, uint32_t level) +{ + this->alg_.lock(); + + this->window_size_ = config.window_size; + this->k_ = config.k; + + this->alg_.unlock(); +} + +void ImageLocalBinarizationAlgNodelet::addNodeDiagnostics(void) +{ +} + +#include "image_local_binarization_alg_nodelet.h" +#include <pluginlib/class_list_macros.h> + +ImageLocalBinNodelet::ImageLocalBinNodelet() +{ + this->dev=NULL; +} + +void ImageLocalBinNodelet::onInit() +{ + this->dev=new ImageLocalBinarizationAlgNodelet(getPrivateNodeHandle()); + // initialize the thread + this->thread_server=CThreadServer::instance(); + this->spin_thread_id=getName() + "_image_local_bin_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); + // start the spin thread + this->thread_server->start_thread(this->spin_thread_id); +} + +void *ImageLocalBinNodelet::spin_thread(void *param) +{ + ImageLocalBinNodelet *nodelet=(ImageLocalBinNodelet *)param; + + nodelet->dev->spin(); + + pthread_exit(NULL); +} + +ImageLocalBinNodelet::~ImageLocalBinNodelet() +{ + // kill the thread + this->thread_server->kill_thread(this->spin_thread_id); + this->thread_server->detach_thread(this->spin_thread_id); + this->thread_server->delete_thread(this->spin_thread_id); + this->spin_thread_id=""; + if(this->dev!=NULL) + delete this->dev; +} + +// parameters are: package, class name, class type, base class type +PLUGINLIB_DECLARE_CLASS(iri_image_local_binarization, ImageLocalBinNodelet, ImageLocalBinNodelet, nodelet::Nodelet); +