Initial commit.

CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(iri_color_calibration)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS message_generation iri_base_algorithm iri_blob_detector cv_bridge image_transport camera_info_manager sensor_msgs dynamic_reconfigure actionlib_msgs iri_ros_tools)
+find_package(Qt4 REQUIRED COMPONENTS QtCore QtGui)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+find_package(iriutils REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   gui.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs dynamic_reconfigure actionlib_msgs # Or other packages containing msgs
+# )
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/ColorCalibration.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+ INCLUDE_DIRS include 
+ LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS message_runtime iri_base_algorithm iri_blob_detector cv_bridge image_transport camera_info_manager sensor_msgs dynamic_reconfigure actionlib_msgs iri_ros_tools
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+include(${QT_USE_FILE})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+file(GLOB QT_FORMS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ui/main_gui.ui)
+file(GLOB QT_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} resources/images.qrc resources/lang.qrc)
+file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/main_gui.h include/image_label.h)
+file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS src/main.cpp src/main_gui.cpp src/image_label.cpp)
+QT4_ADD_RESOURCES(QT_RESOURCES_CPP ${QT_RESOURCES})
+QT4_WRAP_UI(QT_FORMS_HPP ${QT_FORMS})
+QT4_WRAP_CPP(QT_MOC_HPP ${QT_MOC})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+#add_library(gui_module src/gui_module.cpp)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP} src/iri_color_calibration_alg.cpp src/iri_color_calibration_alg_node.cpp)
+
+#add_executable(${PROJECT_NAME}_client src/iri_color_calibration_alg_client.cpp src/iri_color_calibration_alg_node_client.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} )
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES})
+
+#target_link_libraries(${PROJECT_NAME}_client ${catkin_LIBRARIES} )
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+#target_link_libraries(gui_module ${catkin_LIBRARIES})
+#target_link_libraries(gui_module ${iriutils_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} iri_blob_detector_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} dynamic_reconfigure_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+
+install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

cfg/ColorCalibration.cfg

+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='iri_color_calibration'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  0,                               "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+gen.add("fullscreen",  bool_t,  0, "fullscreen",  True)
+
+exit(gen.generate(PACKAGE, "ColorCalibrationAlgorithm", "ColorCalibration"))

include/image_label.h

+#ifndef _IMAGE_LABEL_H
+#define _IMAGE_LABEL_H
+
+#include <QLabel>
+#include <QPoint>
+#include <QWidget>
+#include <QMouseEvent>
+
+class ImageLabel : public QLabel
+{
+  Q_OBJECT
+
+  public:
+    explicit ImageLabel(const QString& text ="", QWidget * parent = 0 );
+    explicit ImageLabel(QWidget * parent = 0 );
+    ~ImageLabel();
+
+  signals:
+    void mousePressed(QPoint &point);
+
+  protected:
+    void mousePressEvent(QMouseEvent *event);
+};
+
+#endif

include/iri_color_calibration_alg.h

+// 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 _color_calibration_alg_h_
+#define _color_calibration_alg_h_
+
+#include <iri_color_calibration/ColorCalibrationConfig.h>
+#include <iri_blob_detector/hsv_filter.h>
+
+//include iri_color_calibration_alg main library
+
+typedef struct
+{
+  unsigned int hue;
+  unsigned int sat;
+  unsigned int val;
+}THSV;
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class ColorCalibrationAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the ColorCalibrationConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;    
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the ColorCalibrationConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef iri_color_calibration::ColorCalibrationConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    ColorCalibrationAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    * 
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void) 
+    { 
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& new_cfg, uint32_t level=0);
+
+    // here define all color_calibration_alg interface methods to retrieve and set
+    // the driver parameters
+    void rgb_to_hsv(unsigned char R,unsigned char G, unsigned char B, unsigned int &H,unsigned int &S, unsigned int &V);
+    void update_filter(std::vector<THSV> &pixels,iri_blob_detector::hsv_filter &filter);
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~ColorCalibrationAlgorithm(void);
+};
+
+#endif

include/iri_color_calibration_alg_node.h

+// 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 _color_calibration_alg_node_h_
+#define _color_calibration_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "iri_color_calibration_alg.h"
+
+// [publisher subscriber headers]
+#include <iri_blob_detector/blob_array.h>
+#include <camera_info_manager/camera_info_manager.h>
+#include <image_transport/image_transport.h>
+// Uncomment to use the openCV <-> ROS bridge
+//#include <cv_bridge/cv_bridge.h>
+#include <sensor_msgs/Image.h>
+
+// [service client headers]
+#include <iri_blob_detector/set_hsv_filter.h>
+#include <dynamic_reconfigure/Reconfigure.h>
+
+// [action server client headers]
+
+// forward declaration of the main windows class
+class MainGui;
+
+typedef struct
+{
+  unsigned int center_x;
+  unsigned int center_y;
+  float size;
+}TBlob;
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class ColorCalibrationAlgNode : public algorithm_base::IriBaseAlgorithm<ColorCalibrationAlgorithm>
+{
+  private:
+    // [publisher attributes]
+
+    // [subscriber attributes]
+    ros::Subscriber blobs_subscriber_;
+    void blobs_callback(const iri_blob_detector::blob_array::ConstPtr& msg);
+    pthread_mutex_t blobs_mutex_;
+    void blobs_mutex_enter(void);
+    void blobs_mutex_exit(void);
+    std::vector<TBlob> blobs;
+
+    image_transport::CameraSubscriber segmented_image_subscriber_;
+    void segmented_image_callback(const sensor_msgs::Image::ConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& info);
+    pthread_mutex_t segmented_image_mutex_;
+    void segmented_image_mutex_enter(void);
+    void segmented_image_mutex_exit(void);
+    sensor_msgs::Image segmented_image;
+    bool segmented_image_received;
+
+    image_transport::CameraSubscriber original_image_subscriber_;
+    void original_image_callback(const sensor_msgs::Image::ConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& info);
+    pthread_mutex_t original_image_mutex_;
+    void original_image_mutex_enter(void);
+    void original_image_mutex_exit(void);
+    sensor_msgs::Image original_image;
+    bool original_image_received;
+
+    // Uncomment to use the openCV <-> ROS bridge
+    //cv_bridge::CvImagePtr cv_image_;
+    image_transport::ImageTransport it;
+
+    // [service attributes]
+
+    // [client attributes]
+    ros::ServiceClient set_filter_client_;
+    iri_blob_detector::set_hsv_filter set_filter_srv_;
+    std::vector<THSV> filter_pixels;
+    pthread_mutex_t filter_mutex_;
+    void filter_mutex_enter(void);
+    void filter_mutex_exit(void);
+    std::string color_id;
+
+    // [action server attributes]
+
+    // [action client attributes]
+    Config config;
+    MainGui *gui;
+  public:
+   /**
+    * \brief Constructor
+    * 
+    * This constructor initializes specific class attributes and all ROS
+    * communications variables to enable message exchange.
+    */
+    ColorCalibrationAlgNode(MainGui *gui);
+
+    void get_image_size(unsigned int &width, unsigned int &height);
+    bool get_original_image(unsigned char* & data, int & width, int & height, std::string & encoding);
+    bool get_segmented_image(unsigned char* & data, int & width, int & height, std::string & encoding);
+    bool get_hsv_pixel(unsigned int row, unsigned int col, unsigned int &hue, unsigned int &sat, unsigned int &val);
+    bool get_rgb_pixel(unsigned int row, unsigned int col, unsigned char &R, unsigned char &G, unsigned char &B);
+
+    void update_size_filter(unsigned int max, unsigned int min);
+    void get_size_filter(unsigned int &max, unsigned int &min);
+    void add_pixel_filter(unsigned int &hue, unsigned int &sat, unsigned int &val);
+    void remove_pixel_filter(unsigned int index);
+    void clear_filter(void);
+    void get_hsv_filter(unsigned int &h_center,unsigned int &h_window,unsigned int &s_center,unsigned int &s_window,unsigned int &v_center,unsigned int &v_window);
+
+    void get_blobs(std::vector<TBlob> &blobs);
+
+    void load_params(const std::string &filename);
+    void save_params(const std::string &filename);
+    void get_color_id(std::string &color_id);
+   /**
+    * \brief Destructor
+    * 
+    * This destructor frees all necessary dynamic memory allocated within this
+    * this class.
+    */
+    ~ColorCalibrationAlgNode(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]
+};
+
+#endif

include/main_gui.h

+#ifndef _MAIN_GUI_H
+#define _MAIN_GUI_H
+
+#include <QWidget>
+#include <QTimer>
+#include "ui_main_gui.h"
+#ifndef Q_MOC_RUN
+  #include "iri_color_calibration_alg_node.h"
+#endif
+
+class MainGui : public QWidget
+{
+  Q_OBJECT
+
+  public:
+    MainGui(int argc, char** argv, QWidget *parent = 0);
+    ~MainGui();
+
+  public Q_SLOTS:
+    void on_image_mousePressed(QPoint &point);
+    void on_delete_all_pixel_clicked(bool status);
+    void on_delete_pixel_clicked(bool status);
+    void on_blob_max_size_textChanged(const QString &new_value);
+    void on_blob_min_size_textChanged(const QString &new_value);
+    void on_load_params_pressed(void);
+    void on_save_params_pressed(void);
+    void timerDone();
+
+  protected:
+    pthread_t thread;
+    static void *rosThread(void *param);
+    void show_filter_params(void);
+
+  public:
+    Ui::color_calibration *ui;
+
+  private:
+    ColorCalibrationAlgNode *node;
+    QTimer *timer;
+    QVector<QRgb> grayscale_color_table;
+};
+
+#endif

launch/node.launch

+<!-- -->
+<launch>
+ 
+  <include file="$(find iri_blob_detector)/launch/usb_cam_tag.launch" />
+ 
+  <node pkg ="iri_color_calibration"
+        type="iri_color_calibration"
+        name="color_calibration"
+        output="screen">
+      <remap from="~/original_image/image_raw" to="/camera/image_raw"/>
+      <remap from="~/original_image/camera_info" to="/camera/camera_info"/>
+      <remap from="~/segmented_image/image_raw" to="/blob/image_out/image_raw"/>
+      <remap from="~/segmented_image/camera_info" to="/blob/image_out/camera_info"/>
+      <remap from="~/set_filter" to="/blob/set_filter"/>
+      <remap from="~/blobs" to="/blob/blobs"/>
+  </node>
+
+</launch>

package.xml

+<?xml version="1.0"?>
+<package>
+  <name>iri_color_calibration</name>
+  <version>0.0.0</version>
+  <description>The iri_color_calibration package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="labrobotica@iri.upc.edu">IRI labrobotica</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/iri_color_calibration</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->