[ros-vision-opencv] 01/07: Imported Upstream version 1.12.2+ds

Leopold Palomo-Avellaneda leo at alaxarxa.net
Sun Oct 23 17:40:58 UTC 2016


This is an automated email from the git hooks/post-receive script.

lepalom-guest pushed a commit to branch master
in repository ros-vision-opencv.

commit 50dab5e0e6e6af023e9b4665bd7f6927757aff93
Author: Leopold Palomo-Avellaneda <leopold.palomo at upc.edu>
Date:   Sat Oct 22 23:27:15 2016 +0200

    Imported Upstream version 1.12.2+ds
---
 .travis.sh                                         |  77 ++++
 .travis.yml                                        |  54 +--
 cv_bridge/CHANGELOG.rst                            |  65 +++
 cv_bridge/include/cv_bridge/cv_bridge.h            |  29 +-
 cv_bridge/package.xml                              |   8 +-
 cv_bridge/python/cv_bridge/__init__.py             |   7 +
 cv_bridge/python/cv_bridge/core.py                 | 117 ++++-
 cv_bridge/src/cv_bridge.cpp                        |  95 +++--
 cv_bridge/src/module.cpp                           |  49 +++
 cv_bridge/src/module_opencv3.cpp                   |   1 +
 cv_bridge/src/pycompat.hpp                         |  70 +++
 cv_bridge/test/CMakeLists.txt                      |   2 +
 cv_bridge/test/conversions.py                      |  85 ++++
 cv_bridge/test/enumerants.py                       |  41 +-
 cv_bridge/test/python_bindings.py                  |  26 ++
 image_geometry/CHANGELOG.rst                       |  27 ++
 image_geometry/CMakeLists.txt                      |   2 +-
 image_geometry/package.xml                         |   9 +-
 image_geometry/rosdoc.yaml                         |   2 +-
 image_geometry/src/image_geometry/cameramodels.py  |   6 +
 image_geometry/src/pinhole_camera_model.cpp        |  19 +-
 image_geometry/test/utest.cpp                      |  91 ++++
 opencv_apps/CHANGELOG.rst                          |  44 --
 opencv_apps/CMakeLists.txt                         | 309 --------------
 opencv_apps/cfg/CamShift.cfg                       |  46 --
 opencv_apps/cfg/ContourMoments.cfg                 |  45 --
 opencv_apps/cfg/ConvexHull.cfg                     |  45 --
 opencv_apps/cfg/EdgeDetection.cfg                  |  50 ---
 opencv_apps/cfg/FBackFlow.cfg                      |  42 --
 opencv_apps/cfg/FaceDetection.cfg                  |  42 --
 opencv_apps/cfg/FindContours.cfg                   |  45 --
 opencv_apps/cfg/GeneralContours.cfg                |  45 --
 opencv_apps/cfg/GoodfeatureTrack.cfg               |  44 --
 opencv_apps/cfg/HoughCircles.cfg                   |  45 --
 opencv_apps/cfg/HoughLines.cfg                     |  48 ---
 opencv_apps/cfg/LKFlow.cfg                         |  42 --
 opencv_apps/cfg/PeopleDetect.cfg                   |  42 --
 opencv_apps/cfg/PhaseCorr.cfg                      |  42 --
 opencv_apps/cfg/SegmentObjects.cfg                 |  42 --
 opencv_apps/cfg/SimpleFlow.cfg                     |  44 --
 opencv_apps/cfg/WatershedSegmentation.cfg          |  42 --
 opencv_apps/msg/Circle.msg                         |   3 -
 opencv_apps/msg/CircleArray.msg                    |   2 -
 opencv_apps/msg/CircleArrayStamped.msg             |   3 -
 opencv_apps/msg/Contour.msg                        |   1 -
 opencv_apps/msg/ContourArray.msg                   |   1 -
 opencv_apps/msg/ContourArrayStamped.msg            |   3 -
 opencv_apps/msg/Face.msg                           |   2 -
 opencv_apps/msg/FaceArray.msg                      |   2 -
 opencv_apps/msg/FaceArrayStamped.msg               |   3 -
 opencv_apps/msg/Flow.msg                           |   2 -
 opencv_apps/msg/FlowArray.msg                      |   1 -
 opencv_apps/msg/FlowArrayStamped.msg               |   2 -
 opencv_apps/msg/FlowStamped.msg                    |   2 -
 opencv_apps/msg/Line.msg                           |   3 -
 opencv_apps/msg/LineArray.msg                      |   1 -
 opencv_apps/msg/LineArrayStamped.msg               |   2 -
 opencv_apps/msg/Moment.msg                         |  34 --
 opencv_apps/msg/MomentArray.msg                    |   1 -
 opencv_apps/msg/MomentArrayStamped.msg             |   2 -
 opencv_apps/msg/Point2D.msg                        |   3 -
 opencv_apps/msg/Point2DArray.msg                   |   1 -
 opencv_apps/msg/Point2DArrayStamped.msg            |   2 -
 opencv_apps/msg/Point2DStamped.msg                 |   3 -
 opencv_apps/msg/Rect.msg                           |   6 -
 opencv_apps/msg/RectArray.msg                      |   1 -
 opencv_apps/msg/RectArrayStamped.msg               |   4 -
 opencv_apps/msg/RotatedRect.msg                    |   3 -
 opencv_apps/msg/RotatedRectArray.msg               |   1 -
 opencv_apps/msg/RotatedRectArrayStamped.msg        |   3 -
 opencv_apps/msg/RotatedRectStamped.msg             |   4 -
 opencv_apps/msg/Size.msg                           |   3 -
 opencv_apps/nodelet_plugins.xml                    |  79 ----
 opencv_apps/package.xml                            |  41 --
 opencv_apps/src/node/camshift.cpp                  |  55 ---
 opencv_apps/src/node/contour_moments.cpp           |  56 ---
 opencv_apps/src/node/convex_hull.cpp               |  56 ---
 opencv_apps/src/node/edge_detection.cpp            |  56 ---
 opencv_apps/src/node/face_detection.cpp            |  55 ---
 opencv_apps/src/node/fback_flow.cpp                |  55 ---
 opencv_apps/src/node/find_contours.cpp             |  56 ---
 opencv_apps/src/node/general_contours.cpp          |  56 ---
 opencv_apps/src/node/goodfeature_track.cpp         |  55 ---
 opencv_apps/src/node/hough_circles.cpp             |  55 ---
 opencv_apps/src/node/hough_lines.cpp               |  55 ---
 opencv_apps/src/node/lk_flow.cpp                   |  55 ---
 opencv_apps/src/node/people_detect.cpp             |  55 ---
 opencv_apps/src/node/phase_corr.cpp                |  55 ---
 opencv_apps/src/node/segment_objects.cpp           |  55 ---
 opencv_apps/src/node/simple_compressed_example.cpp |  55 ---
 opencv_apps/src/node/simple_example.cpp            |  55 ---
 opencv_apps/src/node/simple_flow.cpp               |  55 ---
 opencv_apps/src/node/watershed_segmentation.cpp    |  55 ---
 opencv_apps/src/nodelet/camshift_nodelet.cpp       | 472 ---------------------
 .../src/nodelet/contour_moments_nodelet.cpp        | 310 --------------
 opencv_apps/src/nodelet/convex_hull_nodelet.cpp    | 282 ------------
 opencv_apps/src/nodelet/edge_detection_nodelet.cpp | 321 --------------
 opencv_apps/src/nodelet/face_detection_nodelet.cpp | 271 ------------
 opencv_apps/src/nodelet/fback_flow_nodelet.cpp     | 264 ------------
 opencv_apps/src/nodelet/find_contours_nodelet.cpp  | 279 ------------
 .../src/nodelet/general_contours_nodelet.cpp       | 311 --------------
 .../src/nodelet/goodfeature_track_nodelet.cpp      | 282 ------------
 opencv_apps/src/nodelet/hough_circles_nodelet.cpp  | 295 -------------
 opencv_apps/src/nodelet/hough_lines_nodelet.cpp    | 318 --------------
 opencv_apps/src/nodelet/lk_flow_nodelet.cpp        | 370 ----------------
 opencv_apps/src/nodelet/people_detect_nodelet.cpp  | 264 ------------
 opencv_apps/src/nodelet/phase_corr_nodelet.cpp     | 261 ------------
 .../src/nodelet/segment_objects_nodelet.cpp        | 318 --------------
 .../nodelet/simple_compressed_example_nodelet.cpp  | 121 ------
 opencv_apps/src/nodelet/simple_example_nodelet.cpp | 122 ------
 opencv_apps/src/nodelet/simple_flow_nodelet.cpp    | 297 -------------
 .../src/nodelet/watershed_segmentation_nodelet.cpp | 380 -----------------
 opencv_apps/test/CMakeLists.txt                    |  39 --
 opencv_apps/test/test-camshift.test                |  32 --
 opencv_apps/test/test-contour_moments.test         |  24 --
 opencv_apps/test/test-convex_hull.test             |  24 --
 opencv_apps/test/test-edge_detection.test          |  54 ---
 opencv_apps/test/test-face_detection.test          |  23 -
 opencv_apps/test/test-fback_flow.test              |  23 -
 opencv_apps/test/test-find_contours.test           |  24 --
 opencv_apps/test/test-general_contours.test        |  30 --
 opencv_apps/test/test-goodfeature_track.test       |  24 --
 opencv_apps/test/test-hough_circles.test           |  25 --
 opencv_apps/test/test-hough_lines.test             |  42 --
 opencv_apps/test/test-lk_flow.test                 |  25 --
 opencv_apps/test/test-people_detect.test           |  23 -
 opencv_apps/test/test-phase_corr.test              |  23 -
 opencv_apps/test/test-segment_objects.test         |  24 --
 opencv_apps/test/test-simple_flow.test             |  24 --
 opencv_apps/test/test-watershed_segmentation.test  |  26 --
 opencv_tests/CHANGELOG.rst                         |  41 ++
 opencv_tests/nodes/broadcast.py                    |  53 ++-
 opencv_tests/nodes/rosfacedetect.py                |  48 ++-
 opencv_tests/nodes/source.py                       |  75 ++--
 opencv_tests/package.xml                           |   2 +-
 vision_opencv/CHANGELOG.rst                        |  14 +
 vision_opencv/package.xml                          |   4 +-
 137 files changed, 892 insertions(+), 8652 deletions(-)

diff --git a/.travis.sh b/.travis.sh
new file mode 100755
index 0000000..41c634f
--- /dev/null
+++ b/.travis.sh
@@ -0,0 +1,77 @@
+#!/bin/bash
+
+set -e
+
+function travis_time_start {
+    set +x
+    TRAVIS_START_TIME=$(date +%s%N)
+    TRAVIS_TIME_ID=$(cat /dev/urandom | tr -dc 'a-z0-9' | fold -w 8 | head -n 1)
+    TRAVIS_FOLD_NAME=$1
+    echo -e "\e[0Ktraivs_fold:start:$TRAVIS_FOLD_NAME"
+    echo -e "\e[0Ktraivs_time:start:$TRAVIS_TIME_ID"
+    set -x
+}
+function travis_time_end {
+    set +x
+    _COLOR=${1:-32}
+    TRAVIS_END_TIME=$(date +%s%N)
+    TIME_ELAPSED_SECONDS=$(( ($TRAVIS_END_TIME - $TRAVIS_START_TIME)/1000000000 ))
+    echo -e "traivs_time:end:$TRAVIS_TIME_ID:start=$TRAVIS_START_TIME,finish=$TRAVIS_END_TIME,duration=$(($TRAVIS_END_TIME - $TRAVIS_START_TIME))\n\e[0K"
+    echo -e "traivs_fold:end:$TRAVIS_FOLD_NAME"
+    echo -e "\e[0K\e[${_COLOR}mFunction $TRAVIS_FOLD_NAME takes $(( $TIME_ELAPSED_SECONDS / 60 )) min $(( $TIME_ELAPSED_SECONDS % 60 )) sec\e[0m"
+    set -x
+}
+
+apt-get update -qq && apt-get install -qq -y -q wget sudo lsb-release # for docker 
+
+travis_time_start setup.before_install
+#before_install:
+# Define some config vars.
+# Install ROS
+sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list"
+wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+sudo apt-get update -qq
+# Install ROS
+sudo apt-get install -qq -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+source /opt/ros/$ROS_DISTRO/setup.bash
+# Setup for rosdep
+sudo rosdep init
+rosdep update
+travis_time_end
+
+travis_time_start setup.install
+# Create a catkin workspace with the package under test.
+#install:
+mkdir -p ~/catkin_ws/src
+
+# Add the package under test to the workspace.
+cd ~/catkin_ws/src
+ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
+travis_time_end
+
+travis_time_start setup.before_script
+# Install all dependencies, using wstool and rosdep.
+# wstool looks for a ROSINSTALL_FILE defined in before_install.
+#before_script:
+# source dependencies: install using wstool.
+cd ~/catkin_ws/src
+wstool init
+wstool up
+
+# package depdencies: install using rosdep.
+cd ~/catkin_ws
+rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+travis_time_end
+
+travis_time_start setup.script
+# Compile and test.
+#script:
+source /opt/ros/$ROS_DISTRO/setup.bash
+cd ~/catkin_ws
+catkin build -p1 -j1 --no-status
+catkin run_tests -p1 -j1
+catkin_test_results --all build
+catkin clean -b --yes
+catkin config --install
+catkin build -p1 -j1 --no-status
+travis_time_end
diff --git a/.travis.yml b/.travis.yml
index 83dfb40..0ec6fa5 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -2,64 +2,16 @@ sudo: required
 dist: trusty
 language: generic
 env:
-  - OPENCV_VERSION=2 ROS_DISTRO=indigo
-  - OPENCV_VERSION=3 ROS_DISTRO=indigo
-  - OPENCV_VERSION=2 ROS_DISTRO=jade
-  - OPENCV_VERSION=3 ROS_DISTRO=jade
+  - ROS_DISTRO=kinetic
 # Install system dependencies, namely ROS.
 before_install:
   # Define some config vars.
-  - export ROS_CI_DESKTOP=`lsb_release -cs`  # e.g. [precise|trusty]
   - export CI_SOURCE_PATH=$(pwd)
   - export REPOSITORY_NAME=${PWD##*/}
-  - export ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
-  - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
   - export ROS_PARALLEL_JOBS='-j8 -l6'
-  # Install ROS
-  - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
-  - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-  - sudo apt-get update -qq
-  # Install ROS
-  - sudo apt-get install -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin
-  - source /opt/ros/$ROS_DISTRO/setup.bash
-  # Setup for rosdep
-  - sudo rosdep init
-  - rosdep update
-
-# Create a catkin workspace with the package under test.
-install:
-  - mkdir -p ~/catkin_ws/src
-
-  # Add the package under test to the workspace.
-  - cd ~/catkin_ws/src
-  - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
-
-  - if [ $OPENCV_VERSION == 3 ]; then  sed -i 's at libopencv-dev@opencv3@' $REPOSITORY_NAME/*/package.xml ; fi
-
-# Install all dependencies, using wstool and rosdep.
-# wstool looks for a ROSINSTALL_FILE defined in before_install.
-before_script:
-  # source dependencies: install using wstool.
-  - cd ~/catkin_ws/src
-  - wstool init
-  - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
-  - if [ $OPENCV_VERSION == 3 ]; then wstool set -y image_pipeline --git https://github.com/ros-perception/image_pipeline; fi # need to recompile image_proc
-  - wstool up
-
-  # package depdencies: install using rosdep.
-  - cd ~/catkin_ws
-  - rosdep install -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO
-
-# Compile and test.
 script:
-  - source /opt/ros/$ROS_DISTRO/setup.bash
-  - cd ~/catkin_ws
-  - catkin build -p1 -j1
-  - catkin run_tests -p1 -j1
-  - catkin_test_results --all build
-  - catkin clean -b
-  - catkin config --install
-  - catkin build -p1 -j1
+  - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
+  - docker run --rm -i -v $CI_SOURCE_PATH:$CI_SOURCE_PATH -e "CI_SOURCE_PATH=$CI_SOURCE_PATH" -e "HOME=$HOME" -e "ROS_DISTRO=$ROS_DISTRO" -e "ROS_PARALLEL_JOBS=$ROS_PARALLEL_JOBS" -t ubuntu:xenial sh -c "cd $CI_SOURCE_PATH; ./.travis.sh"
 after_failure:
   - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \;
   - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done
diff --git a/cv_bridge/CHANGELOG.rst b/cv_bridge/CHANGELOG.rst
index 3805a63..96f57d2 100644
--- a/cv_bridge/CHANGELOG.rst
+++ b/cv_bridge/CHANGELOG.rst
@@ -2,6 +2,71 @@
 Changelog for package cv_bridge
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.12.2 (2016-09-24)
+-------------------
+* Specify background label when colorizing label image
+* Adjust to arbitrary image channels like 32FC40
+  Proper fix for `#141 <https://github.com/ros-perception/vision_opencv/issues/141>`_
+* Remove unexpectedly included print statement
+* Contributors: Kentaro Wada, Vincent Rabaud
+
+1.12.1 (2016-07-11)
+-------------------
+* split the conversion tests out of enumerants
+* support is_bigendian in Python
+  Fixes `#114 <https://github.com/ros-perception/vision_opencv/issues/114>`_
+  Also fixes mono16 test
+* Support compressed Images messages in python for indigo
+  - Add cv2_to_comprssed_imgmsg: Convert from cv2 image to compressed image ros msg.
+  - Add comprssed_imgmsg_to_cv2:   Convert the compress message to a new image.
+  - Add compressed image tests.
+  - Add time to msgs (compressed and regular).
+  add enumerants test for compressed image.
+  merge the compressed tests with the regular ones.
+  better comment explanation. I will squash this commit.
+  Fix indentation
+  fix typo mistage: from .imgmsg_to_compressed_cv2 to .compressed_imgmsg_to_cv2.
+  remove cv2.CV_8UC1
+  remove rospy and time depndency.
+  change from IMREAD_COLOR to IMREAD_ANYCOLOR.
+  - make indentaion of 4.
+  - remove space trailer.
+  - remove space from empty lines.
+  - another set of for loops, it will make things easier to track. In that new set,  just have the number of channels in ([],1,3,4) (ignore two for jpg). from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721943
+  - keep the OpenCV error message. from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721013
+  add debug print for test.
+  add case for 4 channels in test.
+  remove 4 channels case from compressed test.
+  add debug print for test.
+  change typo of format.
+  fix typo in format. change from dip to dib.
+  change to IMREAD_ANYCOLOR as python code. (as it should).
+  rename TIFF to tiff
+  Sperate the tests one for regular images and one for compressed.
+  update comment
+* Add CvtColorForDisplayOptions with new colormap param
+* fix doc jobs
+* Add python binding for cv_bridge::cvtColorForDisplay
+* Don't colorize float image as label image
+  This is a bug and image whose encoding is other than 32SC1 should not be
+  colorized. (currently, depth images with 32FC1 is also colorized.)
+* Fix compilation of cv_bridge with opencv3 and python3.
+* Contributors: Kentaro Wada, Maarten de Vries, Vincent Rabaud, talregev
+
+1.12.0 (2016-03-18)
+-------------------
+* depend on OpenCV3 only
+* Contributors: Vincent Rabaud
+
+1.11.12 (2016-03-10)
+--------------------
+* Fix my typo
+* Remove another eval
+  Because `cvtype2_to_dtype_with_channels('8UCimport os; os.system("rm -rf /")')` should never have a chance of happening.
+* Remove eval, and other fixes
+  Also, extend from object, so as not to get a python 2.2-style class, and use the new-style raise statement
+* Contributors: Eric Wieser
+
 1.11.11 (2016-01-31)
 --------------------
 * clean up the doc files
diff --git a/cv_bridge/include/cv_bridge/cv_bridge.h b/cv_bridge/include/cv_bridge/cv_bridge.h
index a3f2033..2b44303 100644
--- a/cv_bridge/include/cv_bridge/cv_bridge.h
+++ b/cv_bridge/include/cv_bridge/cv_bridge.h
@@ -60,7 +60,7 @@ typedef boost::shared_ptr<CvImage const> CvImageConstPtr;
 
 //from: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
 typedef enum {
-	BMP, DIP,
+	BMP, DIB,
 	JPG, JPEG, JPE,
 	JP2,
 	PNG,
@@ -247,6 +247,21 @@ CvImageConstPtr toCvShare(const sensor_msgs::Image& source,
 CvImagePtr cvtColor(const CvImageConstPtr& source,
                     const std::string& encoding);
 
+struct CvtColorForDisplayOptions {
+  CvtColorForDisplayOptions() :
+    do_dynamic_scaling(false),
+    min_image_value(0.0),
+    max_image_value(0.0),
+    colormap(-1),
+    bg_label(-1) {}
+  bool do_dynamic_scaling;
+  double min_image_value;
+  double max_image_value;
+  int colormap;
+  int bg_label;
+};
+
+
 /**
  * \brief Converts an immutable sensor_msgs::Image message to another CvImage for display purposes,
  * using practical conversion rules if needed.
@@ -270,17 +285,17 @@ CvImagePtr cvtColor(const CvImageConstPtr& source,
  * \param source   A shared_ptr to a sensor_msgs::Image message
  * \param encoding Either an encoding string that returns true in sensor_msgs::image_encodings::isColor
  * isMono or the empty string as explained above.
- * \param do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value
+ * \param options (cv_bridge::CvtColorForDisplayOptions) Options to convert the source image with.
+ * - do_dynamic_scaling If true, the image is dynamically scaled between its minimum and maximum value
  * before being converted to its final encoding.
- * \param min_image_value Independently from do_dynamic_scaling, if min_image_value and max_image_value are
+ * - min_image_value Independently from do_dynamic_scaling, if min_image_value and max_image_value are
  * different, the image is scaled between these two values before being converted to its final encoding.
- * \param max_image_value Maximum image value
+ * - max_image_value Maximum image value
+ * - colormap Colormap which the source image converted with.
  */
 CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
                                    const std::string& encoding = std::string(),
-                                   bool do_dynamic_scaling = false,
-                                   double min_image_value = 0.0,
-                                   double max_image_value = 0.0);
+                                   const CvtColorForDisplayOptions options = CvtColorForDisplayOptions());
 
 /**
  * \brief Get the OpenCV type enum corresponding to the encoding.
diff --git a/cv_bridge/package.xml b/cv_bridge/package.xml
index a5df57c..7652d37 100644
--- a/cv_bridge/package.xml
+++ b/cv_bridge/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>cv_bridge</name>
-  <version>1.11.11</version>
+  <version>1.12.2</version>
   <description>
     This contains CvBridge, which converts between ROS
     Image messages and OpenCV images.
@@ -20,16 +20,14 @@
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
-  <build_depend>libopencv-dev</build_depend>
+  <build_depend>opencv3</build_depend>
   <build_depend>python</build_depend>
-  <build_depend>python-opencv</build_depend>
   <build_depend>rosconsole</build_depend>
   <build_depend>sensor_msgs</build_depend>
 
   <exec_depend>boost</exec_depend>
-  <exec_depend>libopencv-dev</exec_depend>
+  <exec_depend>opencv3</exec_depend>
   <exec_depend>python</exec_depend>
-  <exec_depend>python-opencv</exec_depend>
   <exec_depend>rosconsole</exec_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
 
diff --git a/cv_bridge/python/cv_bridge/__init__.py b/cv_bridge/python/cv_bridge/__init__.py
index d37a7b4..5189c1a 100644
--- a/cv_bridge/python/cv_bridge/__init__.py
+++ b/cv_bridge/python/cv_bridge/__init__.py
@@ -1 +1,8 @@
 from .core import CvBridge, CvBridgeError
+
+# python bindings
+try:
+    # This try is just to satisfy doc jobs that are built differently.
+    from cv_bridge.boost.cv_bridge_boost import cvtColorForDisplay, getCvType
+except ImportError:
+    pass
diff --git a/cv_bridge/python/cv_bridge/core.py b/cv_bridge/python/cv_bridge/core.py
index 9549c9c..b392989 100644
--- a/cv_bridge/python/cv_bridge/core.py
+++ b/cv_bridge/python/cv_bridge/core.py
@@ -1,6 +1,7 @@
 # Software License Agreement (BSD License)
 #
 # Copyright (c) 2011, Willow Garage, Inc.
+# Copyright (c) 2016, Tal Regev.
 # All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
@@ -31,6 +32,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 import sensor_msgs.msg
+import sys
+
 
 class CvBridgeError(TypeError):
     """
@@ -38,7 +41,8 @@ class CvBridgeError(TypeError):
     """
     pass
 
-class CvBridge:
+
+class CvBridge(object):
     """
     The CvBridge is an object that converts between OpenCV Images and ROS Image messages.
 
@@ -53,6 +57,8 @@ class CvBridge:
            >>> im = np.ndarray(shape=(480, 640, n_channels), dtype=dtype)
            >>> msg = br.cv2_to_imgmsg(im)  # Convert the image to a message
            >>> im2 = br.imgmsg_to_cv2(msg) # Convert the message to a new image
+           >>> cmprsmsg = br.cv2_to_compressed_imgmsg(im)  # Convert the image to a compress message
+           >>> im22 = br.compressed_imgmsg_to_cv2(msg) # Convert the compress message to a new image
            >>> cv2.imwrite("this_was_a_message_briefly.png", im2)
 
     """
@@ -60,24 +66,26 @@ class CvBridge:
     def __init__(self):
         import cv2
         self.cvtype_to_name = {}
+        self.cvdepth_to_numpy_depth = {cv2.CV_8U: 'uint8', cv2.CV_8S: 'int8', cv2.CV_16U: 'uint16',
+                                       cv2.CV_16S: 'int16', cv2.CV_32S:'int32', cv2.CV_32F:'float32',
+                                       cv2.CV_64F: 'float64'}
 
-        for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F" ]:
-            for c in [1,2,3,4]:
+        for t in ["8U", "8S", "16U", "16S", "32S", "32F", "64F"]:
+            for c in [1, 2, 3, 4]:
                 nm = "%sC%d" % (t, c)
-                self.cvtype_to_name[eval("cv2.CV_%s" % nm)] = nm
+                self.cvtype_to_name[getattr(cv2, "CV_%s" % nm)] = nm
 
-        self.numpy_type_to_cvtype = {'uint8':'8U', 'int8':'8S', 'uint16':'16U',
-                                        'int16':'16S', 'int32':'32S', 'float32':'32F',
-                                        'float64':'64F'}
+        self.numpy_type_to_cvtype = {'uint8': '8U', 'int8': '8S', 'uint16': '16U',
+                                        'int16': '16S', 'int32': '32S', 'float32': '32F',
+                                        'float64': '64F'}
         self.numpy_type_to_cvtype.update(dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()))
 
     def dtype_with_channels_to_cvtype2(self, dtype, n_channels):
         return '%sC%d' % (self.numpy_type_to_cvtype[dtype.name], n_channels)
 
     def cvtype2_to_dtype_with_channels(self, cvtype):
-        import re
-        vals = re.split('(.+)C(.+)', self.cvtype_to_name[cvtype])
-        return self.numpy_type_to_cvtype[vals[1]], eval(vals[2])
+        from cv_bridge.boost.cv_bridge_boost import CV_MAT_CNWrap, CV_MAT_DEPTHWrap
+        return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype)
 
     def encoding_to_cvtype2(self, encoding):
         from cv_bridge.boost.cv_bridge_boost import getCvType
@@ -90,6 +98,46 @@ class CvBridge:
     def encoding_to_dtype_with_channels(self, encoding):
         return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding))
 
+    def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding = "passthrough"):
+        """
+        Convert a sensor_msgs::CompressedImage message to an OpenCV :cpp:type:`cv::Mat`.
+
+        :param cmprs_img_msg:   A :cpp:type:`sensor_msgs::CompressedImage` message
+        :param desired_encoding:  The encoding of the image data, one of the following strings:
+
+           * ``"passthrough"``
+           * one of the standard strings in sensor_msgs/image_encodings.h
+
+        :rtype: :cpp:type:`cv::Mat`
+        :raises CvBridgeError: when conversion is not possible.
+
+        If desired_encoding is ``"passthrough"``, then the returned image has the same format as img_msg.
+        Otherwise desired_encoding must be one of the standard image encodings
+
+        This function returns an OpenCV :cpp:type:`cv::Mat` message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
+
+        If the image only has one channel, the shape has size 2 (width and height)
+        """
+        import cv2
+        import numpy as np
+
+        str_msg = cmprs_img_msg.data
+        buf = np.ndarray(shape=(1, len(str_msg)),
+                          dtype=np.uint8, buffer=cmprs_img_msg.data)
+        im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR)
+
+        if desired_encoding == "passthrough":
+            return im
+
+        from cv_bridge.boost.cv_bridge_boost import cvtColor2
+
+        try:
+            res = cvtColor2(im, "bgr8", desired_encoding)
+        except RuntimeError as e:
+            raise CvBridgeError(e)
+
+        return res
+
     def imgmsg_to_cv2(self, img_msg, desired_encoding = "passthrough"):
         """
         Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`.
@@ -113,12 +161,17 @@ class CvBridge:
         import cv2
         import numpy as np
         dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding)
+        dtype = np.dtype(dtype)
+        dtype = dtype.newbyteorder('>' if img_msg.is_bigendian else '<')
         if n_channels == 1:
             im = np.ndarray(shape=(img_msg.height, img_msg.width),
                            dtype=dtype, buffer=img_msg.data)
         else:
             im = np.ndarray(shape=(img_msg.height, img_msg.width, n_channels),
                            dtype=dtype, buffer=img_msg.data)
+        # If the byt order is different between the message and the system.
+        if img_msg.is_bigendian == (sys.byteorder == 'little'):
+            im = im.byteswap().newbyteorder()
 
         if desired_encoding == "passthrough":
             return im
@@ -132,6 +185,43 @@ class CvBridge:
 
         return res
 
+    def cv2_to_compressed_imgmsg(self, cvim, dst_format = "jpg"):
+        """
+        Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::CompressedImage message.
+
+        :param cvim:      An OpenCV :cpp:type:`cv::Mat`
+        :param dst_format:  The format of the image data, one of the following strings:
+
+           * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html
+           * from http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
+           * bmp, dib
+           * jpeg, jpg, jpe
+           * jp2
+           * png
+           * pbm, pgm, ppm
+           * sr, ras
+           * tiff, tif
+
+        :rtype:           A sensor_msgs.msg.CompressedImage message
+        :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``format``
+
+
+        This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
+        """
+        import cv2
+        import numpy as np
+        if not isinstance(cvim, (np.ndarray, np.generic)):
+            raise TypeError('Your input type is not a numpy array')
+        cmprs_img_msg = sensor_msgs.msg.CompressedImage()
+        cmprs_img_msg.format = dst_format
+        ext_format = '.' + dst_format
+        try:
+            cmprs_img_msg.data = np.array(cv2.imencode(ext_format, cvim)[1]).tostring()
+        except RuntimeError as e:
+            raise CvBridgeError(e)
+
+        return cmprs_img_msg
+
     def cv2_to_imgmsg(self, cvim, encoding = "passthrough"):
         """
         Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.
@@ -152,7 +242,7 @@ class CvBridge:
         """
         import cv2
         import numpy as np
-        if not isinstance(cvim, (np.ndarray, np.generic) ):
+        if not isinstance(cvim, (np.ndarray, np.generic)):
             raise TypeError('Your input type is not a numpy array')
         img_msg = sensor_msgs.msg.Image()
         img_msg.height = cvim.shape[0]
@@ -167,7 +257,10 @@ class CvBridge:
             img_msg.encoding = encoding
             # Verify that the supplied encoding is compatible with the type of the OpenCV image
             if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type:
-              raise CvBridgeError, "encoding specified as %s, but image has incompatible type %s" % (encoding, cv_type)
+                raise CvBridgeError("encoding specified as %s, but image has incompatible type %s" % (encoding, cv_type))
+        if cvim.dtype.byteorder == '>':
+            img_msg.is_bigendian = True
         img_msg.data = cvim.tostring()
         img_msg.step = len(img_msg.data) / img_msg.height
+
         return img_msg
diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp
index e284fd3..4919078 100644
--- a/cv_bridge/src/cv_bridge.cpp
+++ b/cv_bridge/src/cv_bridge.cpp
@@ -38,6 +38,7 @@
 #include <map>
 
 #include <boost/make_shared.hpp>
+#include <boost/regex.hpp>
 
 #include <opencv2/imgproc/imgproc.hpp>
 
@@ -52,6 +53,23 @@ namespace enc = sensor_msgs::image_encodings;
 
 namespace cv_bridge {
 
+static int depthStrToInt(const std::string depth) {
+  if (depth == "8U") {
+    return 0;
+  } else if (depth == "8S") {
+    return 1;
+  } else if (depth == "16U") {
+    return 2;
+  } else if (depth == "16S") {
+    return 3;
+  } else if (depth == "32S") {
+    return 4;
+  } else if (depth == "32F") {
+    return 5;
+  }
+  return 6;
+}
+
 int getCvType(const std::string& encoding)
 {
   // Check for the most common encodings first
@@ -80,26 +98,17 @@ int getCvType(const std::string& encoding)
   if (encoding == enc::YUV422) return CV_8UC2;
 
   // Check all the generic content encodings
-#define CHECK_ENCODING(code)                            \
-  if (encoding == enc::TYPE_##code) return CV_##code    \
-  /***/
-#define CHECK_CHANNEL_TYPE(t)                   \
-  CHECK_ENCODING(t##1);                         \
-  CHECK_ENCODING(t##2);                         \
-  CHECK_ENCODING(t##3);                         \
-  CHECK_ENCODING(t##4);                         \
-  /***/
-
-  CHECK_CHANNEL_TYPE(8UC);
-  CHECK_CHANNEL_TYPE(8SC);
-  CHECK_CHANNEL_TYPE(16UC);
-  CHECK_CHANNEL_TYPE(16SC);
-  CHECK_CHANNEL_TYPE(32SC);
-  CHECK_CHANNEL_TYPE(32FC);
-  CHECK_CHANNEL_TYPE(64FC);
-
-#undef CHECK_CHANNEL_TYPE
-#undef CHECK_ENCODING
+  boost::cmatch m;
+
+  if (boost::regex_match(encoding.c_str(), m,
+        boost::regex("(8U|8S|16U|16S|32S|32F|64F)C([0-9]+)"))) {
+    return CV_MAKETYPE(depthStrToInt(m[1].str()), atoi(m[2].str().c_str()));
+  }
+
+  if (boost::regex_match(encoding.c_str(), m,
+        boost::regex("(8U|8S|16U|16S|32S|32F|64F)"))) {
+    return CV_MAKETYPE(depthStrToInt(m[1].str()), 1);
+  }
 
   throw Exception("Unrecognized image encoding [" + encoding + "]");
 }
@@ -431,7 +440,7 @@ cv::Mat matFromImage(const sensor_msgs::CompressedImage& source)
     cv::Mat jpegData(1,source.data.size(),CV_8UC1);
     jpegData.data     = const_cast<uchar*>(&source.data[0]);
     cv::InputArray data(jpegData);
-    cv::Mat bgrMat     = cv::imdecode(data,cv::IMREAD_COLOR);
+    cv::Mat bgrMat     = cv::imdecode(data,cv::IMREAD_ANYCOLOR);
     return bgrMat;
 }
 
@@ -445,8 +454,8 @@ sensor_msgs::CompressedImagePtr CvImage::toCompressedImageMsg(const Format dst_f
 std::string getFormat(Format format) {
 
 	switch (format) {
-		case DIP:
-			return "dip";
+		case DIB:
+			return "dib";
 		case BMP:
 			return "bmp";
 		case JPG:
@@ -517,10 +526,11 @@ CvImagePtr toCvCopy(const sensor_msgs::CompressedImage& source,
 
 CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
                                    const std::string& encoding_out,
-                                   bool do_dynamic_scaling,
-                                   double min_image_value,
-                                   double max_image_value)
+                                   const CvtColorForDisplayOptions options)
 {
+  double min_image_value = options.min_image_value;
+  double max_image_value = options.max_image_value;
+
   if (!source)
     throw Exception("cv_bridge.cvtColorForDisplay() called with empty image.");
   // let's figure out what to do with the empty encoding
@@ -532,10 +542,10 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
       // Let's decide upon an output format
       if (enc::numChannels(source->encoding) == 1)
       {
-        if ((enc::bitDepth(source->encoding) == 8) ||
-            (enc::bitDepth(source->encoding) == 16))
-          encoding = enc::MONO8;
-        else if (enc::bitDepth(source->encoding) == 32)
+        if ((source->encoding == enc::TYPE_32SC1) ||
+            (enc::bitDepth(source->encoding) == 8) ||
+            (enc::bitDepth(source->encoding) == 16) ||
+            (enc::bitDepth(source->encoding) == 32))
           encoding = enc::BGR8;
         else
           throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
@@ -566,7 +576,7 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
 
   // Convert label to bgr image
   if (encoding == sensor_msgs::image_encodings::BGR8 &&
-      sensor_msgs::image_encodings::bitDepth(source->encoding) == 32)
+      source->encoding == enc::TYPE_32SC1)
   {
     CvImagePtr result(new CvImage());
     result->header = source->header;
@@ -575,7 +585,7 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
     for (size_t j = 0; j < source->image.rows; ++j) {
       for (size_t i = 0; i < source->image.cols; ++i) {
         int label = source->image.at<int>(j, i);
-        if (label == -1) {  // background label
+        if (label == options.bg_label) {  // background label
           result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
         }
         else
@@ -590,7 +600,7 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
   }
 
   // Perform scaling if asked for
-  if (do_dynamic_scaling)
+  if (options.do_dynamic_scaling)
   {
     cv::minMaxLoc(source->image, &min_image_value, &max_image_value);
     if (min_image_value == max_image_value)
@@ -614,12 +624,19 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
   {
     if (enc::numChannels(source->encoding) != 1)
       throw Exception("cv_bridge.cvtColorForDisplay() scaling for images with more than one channel is unsupported");
-    CvImagePtr img_scaled_8u(new CvImage());
-    img_scaled_8u->header = source->header;
-    img_scaled_8u->encoding = enc::MONO8;
-    cv::Mat(source->image-min_image_value).convertTo(img_scaled_8u->image, CV_8UC1, 255.0 /
-      (max_image_value - min_image_value));
-    return cvtColor(img_scaled_8u, encoding);
+    CvImagePtr img_scaled(new CvImage());
+    img_scaled->header = source->header;
+    if (options.colormap == -1) {
+      img_scaled->encoding = enc::MONO8;
+      cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC1, 255.0 /
+        (max_image_value - min_image_value));
+    } else {
+      img_scaled->encoding = enc::BGR8;
+      cv::Mat(source->image-min_image_value).convertTo(img_scaled->image, CV_8UC3, 255.0 /
+        (max_image_value - min_image_value));
+      cv::applyColorMap(img_scaled->image, img_scaled->image, options.colormap);
+    }
+    return cvtColor(img_scaled, encoding);
   }
 
   // If no color conversion is possible, we must "guess" the input format
diff --git a/cv_bridge/src/module.cpp b/cv_bridge/src/module.cpp
index 3506ebf..c123198 100644
--- a/cv_bridge/src/module.cpp
+++ b/cv_bridge/src/module.cpp
@@ -50,6 +50,40 @@ cvtColor2Wrap(bp::object obj_in, const std::string & encoding_in, const std::str
   return bp::object(boost::python::handle<>(pyopencv_from(mat)));
 }
 
+bp::object
+cvtColorForDisplayWrap(bp::object obj_in,
+                       const std::string & encoding_in,
+                       const std::string & encoding_out,
+                       bool do_dynamic_scaling = false,
+                       double min_image_value = 0.0,
+                       double max_image_value = 0.0) {
+  // Convert the Python input to an image
+  cv::Mat mat_in;
+  convert_to_CvMat2(obj_in.ptr(), mat_in);
+
+  cv_bridge::CvImagePtr cv_image(new cv_bridge::CvImage(std_msgs::Header(), encoding_in, mat_in));
+
+  cv_bridge::CvtColorForDisplayOptions options;
+  options.do_dynamic_scaling = do_dynamic_scaling;
+  options.min_image_value = min_image_value;
+  options.max_image_value = max_image_value;
+  cv::Mat mat = cv_bridge::cvtColorForDisplay(/*source=*/cv_image,
+                                              /*encoding_out=*/encoding_out,
+                                              /*options=*/options)->image;
+
+  return bp::object(boost::python::handle<>(pyopencv_from(mat)));
+}
+
+BOOST_PYTHON_FUNCTION_OVERLOADS(cvtColorForDisplayWrap_overloads, cvtColorForDisplayWrap, 3, 6)
+
+int CV_MAT_CNWrap(int i) {
+  return CV_MAT_CN(i);
+}
+
+int CV_MAT_DEPTHWrap(int i) {
+  return CV_MAT_DEPTH(i);
+}
+
 BOOST_PYTHON_MODULE(cv_bridge_boost)
 {
   do_numpy_import();
@@ -58,4 +92,19 @@ BOOST_PYTHON_MODULE(cv_bridge_boost)
   // Wrap the function to get encodings as OpenCV types
   boost::python::def("getCvType", cv_bridge::getCvType);
   boost::python::def("cvtColor2", cvtColor2Wrap);
+  boost::python::def("CV_MAT_CNWrap", CV_MAT_CNWrap);
+  boost::python::def("CV_MAT_DEPTHWrap", CV_MAT_DEPTHWrap);
+  boost::python::def("cvtColorForDisplay", cvtColorForDisplayWrap,
+                     cvtColorForDisplayWrap_overloads(
+                       boost::python::args("source", "encoding_in", "encoding_out", "do_dynamic_scaling",
+                                           "min_image_value", "max_image_value"),
+                       "Convert image to display with specified encodings.\n\n"
+                       "Args:\n"
+                       "  - source (numpy.ndarray): input image\n"
+                       "  - encoding_in (str): input image encoding\n"
+                       "  - encoding_out (str): encoding to which the image conveted\n"
+                       "  - do_dynamic_scaling (bool): flag to do dynamic scaling with min/max value\n"
+                       "  - min_image_value (float): minimum pixel value for dynamic scaling\n"
+                       "  - max_image_value (float): maximum pixel value for dynamic scaling\n"
+                     ));
 }
diff --git a/cv_bridge/src/module_opencv3.cpp b/cv_bridge/src/module_opencv3.cpp
index 4d9a300..68c1b20 100644
--- a/cv_bridge/src/module_opencv3.cpp
+++ b/cv_bridge/src/module_opencv3.cpp
@@ -6,6 +6,7 @@
 
 #include "opencv2/opencv_modules.hpp"
 
+#include "pycompat.hpp"
 
 static PyObject* opencv_error = 0;
 
diff --git a/cv_bridge/src/pycompat.hpp b/cv_bridge/src/pycompat.hpp
new file mode 100644
index 0000000..f4ebea6
--- /dev/null
+++ b/cv_bridge/src/pycompat.hpp
@@ -0,0 +1,70 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not 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 Intel Corporation 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.
+//
+//M*/
+
+// Defines for Python 2/3 compatibility.
+#ifndef __PYCOMPAT_HPP__
+#define __PYCOMPAT_HPP__
+
+#if PY_MAJOR_VERSION >= 3
+// Python3 treats all ints as longs, PyInt_X functions have been removed.
+#define PyInt_Check PyLong_Check
+#define PyInt_CheckExact PyLong_CheckExact
+#define PyInt_AsLong PyLong_AsLong
+#define PyInt_AS_LONG PyLong_AS_LONG
+#define PyInt_FromLong PyLong_FromLong
+#define PyNumber_Int PyNumber_Long
+
+// Python3 strings are unicode, these defines mimic the Python2 functionality.
+#define PyString_Check PyUnicode_Check
+#define PyString_FromString PyUnicode_FromString
+#define PyString_FromStringAndSize PyUnicode_FromStringAndSize
+#define PyString_Size PyUnicode_GET_SIZE
+
+// PyUnicode_AsUTF8 isn't available until Python 3.3
+#if (PY_VERSION_HEX < 0x03030000)
+#define PyString_AsString _PyUnicode_AsString
+#else
+#define PyString_AsString PyUnicode_AsUTF8
+#endif
+#endif
+
+#endif // END HEADER GUARD
diff --git a/cv_bridge/test/CMakeLists.txt b/cv_bridge/test/CMakeLists.txt
index c933afa..6cd6a8c 100644
--- a/cv_bridge/test/CMakeLists.txt
+++ b/cv_bridge/test/CMakeLists.txt
@@ -11,3 +11,5 @@ target_link_libraries(${PROJECT_NAME}-utest
 )
 
 catkin_add_nosetests(enumerants.py)
+catkin_add_nosetests(conversions.py)
+catkin_add_nosetests(python_bindings.py)
diff --git a/cv_bridge/test/conversions.py b/cv_bridge/test/conversions.py
new file mode 100644
index 0000000..ab20cd7
--- /dev/null
+++ b/cv_bridge/test/conversions.py
@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+import rostest
+import unittest
+
+import numpy as np
+
+import sensor_msgs.msg
+
+from cv_bridge import CvBridge, CvBridgeError
+
+class TestConversions(unittest.TestCase):
+
+    def test_mono16_cv2(self):
+        import numpy as np
+        br = CvBridge()
+        im = np.uint8(np.random.randint(0, 255, size=(480, 640, 3)))
+        self.assertRaises(CvBridgeError, lambda: br.imgmsg_to_cv2(br.cv2_to_imgmsg(im), "mono16"))
+        br.imgmsg_to_cv2(br.cv2_to_imgmsg(im,"rgb8"), "mono16")
+
+    def test_encode_decode_cv2(self):
+        import cv2
+        import numpy as np
+        fmts = [cv2.CV_8U, cv2.CV_8S, cv2.CV_16U, cv2.CV_16S, cv2.CV_32S, cv2.CV_32F, cv2.CV_64F]
+
+        cvb_en = CvBridge()
+        cvb_de = CvBridge()
+
+        for w in range(100, 800, 100):
+            for h in range(100, 800, 100):
+                for f in fmts:
+                    for channels in ([], 1, 2, 3, 4, 5):
+                        if channels == []:
+                            original = np.uint8(np.random.randint(0, 255, size=(h, w)))
+                        else:
+                            original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
+                        rosmsg = cvb_en.cv2_to_imgmsg(original)
+                        newimg = cvb_de.imgmsg_to_cv2(rosmsg)
+
+                        self.assert_(original.dtype == newimg.dtype)
+                        if channels == 1:
+                            # in that case, a gray image has a shape of size 2
+                            self.assert_(original.shape[:2] == newimg.shape[:2])
+                        else:
+                            self.assert_(original.shape == newimg.shape)
+                        self.assert_(len(original.tostring()) == len(newimg.tostring()))
+
+    def test_encode_decode_cv2_compressed(self):
+        import numpy as np
+        # from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags)
+        formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm",
+                   "jp2", "sr", "ras", "tif", "tiff"]  # this formats rviz is not support
+
+        cvb_en = CvBridge()
+        cvb_de = CvBridge()
+
+        for w in range(100, 800, 100):
+            for h in range(100, 800, 100):
+                for f in formats:
+                    for channels in ([], 1, 3):
+                        if channels == []:
+                            original = np.uint8(np.random.randint(0, 255, size=(h, w)))
+                        else:
+                            original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
+                        compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f)
+                        newimg          = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg)
+                        self.assert_(original.dtype == newimg.dtype)
+                        if channels == 1:
+                            # in that case, a gray image has a shape of size 2
+                            self.assert_(original.shape[:2] == newimg.shape[:2])
+                        else:
+                            self.assert_(original.shape == newimg.shape)
+                        self.assert_(len(original.tostring()) == len(newimg.tostring()))
+
+    def test_endianness(self):
+        br = CvBridge()
+        dtype = np.dtype('int32')
+        # Set to big endian.
+        dtype = dtype.newbyteorder('>')
+        img = np.random.randint(0, 255, size=(30, 40))
+        msg = br.cv2_to_imgmsg(img.astype(dtype))
+        self.assert_(msg.is_bigendian == True)
+        self.assert_((br.imgmsg_to_cv2(msg) == img).all())
+
+if __name__ == '__main__':
+    rosunit.unitrun('opencv_tests', 'conversions', TestConversions)
diff --git a/cv_bridge/test/enumerants.py b/cv_bridge/test/enumerants.py
index 236cf76..bdcc7a8 100644
--- a/cv_bridge/test/enumerants.py
+++ b/cv_bridge/test/enumerants.py
@@ -2,9 +2,12 @@
 import rostest
 import unittest
 
+import numpy as np
+import cv2
+
 import sensor_msgs.msg
 
-from cv_bridge import CvBridge, CvBridgeError
+from cv_bridge import CvBridge, CvBridgeError, getCvType
 
 class TestEnumerants(unittest.TestCase):
 
@@ -28,31 +31,9 @@ class TestEnumerants(unittest.TestCase):
         bridge_.cv2_to_imgmsg(cvim, "rgb8")
         bridge_.cv2_to_imgmsg(cvim, "bgr8")
 
-    def test_encode_decode_cv2(self):
-        import cv2
-        import numpy as np
-        fmts = [ cv2.CV_8U, cv2.CV_8S, cv2.CV_16U, cv2.CV_16S, cv2.CV_32S, cv2.CV_32F, cv2.CV_64F ]
-
-        cvb_en = CvBridge()
-        cvb_de = CvBridge()
-
-        for w in range(100, 800, 100):
-            for h in range(100, 800, 100):
-                for f in fmts:
-                    for channels in ([],1,2,3,4):
-                        if channels == []:
-                            original = np.uint8(np.random.randint(0, 255, size=(h, w)))
-                        else:
-                            original = np.uint8(np.random.randint(0, 255, size=(h, w, channels)))
-                        rosmsg = cvb_en.cv2_to_imgmsg(original)
-                        newimg = cvb_de.imgmsg_to_cv2(rosmsg)
-                        self.assert_(original.dtype == newimg.dtype)
-                        if channels == 1:
-                            # in that case, a gray image has a shape of size 2
-                            self.assert_(original.shape[:2] == newimg.shape[:2])
-                        else:
-                            self.assert_(original.shape == newimg.shape)
-                        self.assert_(len(original.tostring()) == len(newimg.tostring()))
+        self.assertRaises(getCvType("32FC4") == cv2.CV_8UC4)
+        self.assertRaises(getCvType("8UC1") == cv2.CV_8UC1)
+        self.assertRaises(getCvType("8U") == cv2.CV_8UC1)
 
     def test_numpy_types(self):
         import cv2
@@ -62,11 +43,5 @@ class TestEnumerants(unittest.TestCase):
         if hasattr(cv2, 'cv'):
             self.assertRaises(TypeError, lambda: bridge_.cv2_to_imgmsg(cv2.cv(), "rgba8"))
 
-    def test_mono16_cv2(self):
-        import numpy as np
-        br = CvBridge()
-        im = np.uint8(np.random.randint(0, 255, size=(480, 640, 3)))
-        msg = br.cv2_to_imgmsg(im)
-
 if __name__ == '__main__':
-    rostest.unitrun('opencv_tests', 'enumerants', TestEnumerants)
+    rosunit.unitrun('opencv_tests', 'enumerants', TestEnumerants)
diff --git a/cv_bridge/test/python_bindings.py b/cv_bridge/test/python_bindings.py
new file mode 100644
index 0000000..16d4e28
--- /dev/null
+++ b/cv_bridge/test/python_bindings.py
@@ -0,0 +1,26 @@
+from nose.tools import assert_equal
+import numpy as np
+
+import cv_bridge
+
+
+def test_cvtColorForDisplay():
+    # convert label image to display
+    label = np.zeros((480, 640), dtype=np.int32)
+    height, width = label.shape[:2]
+    label_value = 0
+    grid_num_y, grid_num_x = 3, 4
+    for grid_row in xrange(grid_num_y):
+        grid_size_y = height / grid_num_y
+        min_y = grid_size_y * grid_row
+        max_y = min_y + grid_size_y
+        for grid_col in xrange(grid_num_x):
+            grid_size_x = width / grid_num_x
+            min_x = grid_size_x * grid_col
+            max_x = min_x + grid_size_x
+            label[min_y:max_y, min_x:max_x] = label_value
+            label_value += 1
+    label_viz = cv_bridge.cvtColorForDisplay(label, '32SC1', 'bgr8')
+    assert_equal(label_viz.dtype, np.uint8)
+    assert_equal(label_viz.min(), 0)
+    assert_equal(label_viz.max(), 255)
diff --git a/image_geometry/CHANGELOG.rst b/image_geometry/CHANGELOG.rst
index b0158d5..acef786 100644
--- a/image_geometry/CHANGELOG.rst
+++ b/image_geometry/CHANGELOG.rst
@@ -2,6 +2,33 @@
 Changelog for package image_geometry
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.12.2 (2016-09-24)
+-------------------
+* Fix "stdlib.h: No such file or directory" errors in GCC-6
+  Including '-isystem /usr/include' breaks building with GCC-6.
+  See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129
+* Merge pull request `#142 <https://github.com/ros-perception/vision_opencv/issues/142>`_ from YuOhara/remap_with_nan_border_value
+  remap with nan border if mat value is float or double
+* remap with nan border if mat value is float or double
+* Contributors: Hodorgasm, Vincent Rabaud, YuOhara
+
+1.12.1 (2016-07-11)
+-------------------
+* Add fullResolution getter to PinholeCameraModel
+* add a missing dependency when building the doc
+* fix sphinx doc path
+* Contributors: Jacob Panikulam, Vincent Rabaud
+
+1.12.0 (2016-03-18)
+-------------------
+* depend on OpenCV3 only
+* Contributors: Vincent Rabaud
+
+1.11.12 (2016-03-10)
+--------------------
+* issue `#117 <https://github.com/ros-perception/vision_opencv/issues/117>`_ pull request `#118 <https://github.com/ros-perception/vision_opencv/issues/118>`_ check all distortion coefficients to see if rectification ought to be done
+* Contributors: Lucas Walter
+
 1.11.11 (2016-01-31)
 --------------------
 * clean up the doc files
diff --git a/image_geometry/CMakeLists.txt b/image_geometry/CMakeLists.txt
index 3c9cb30..e7c9eea 100644
--- a/image_geometry/CMakeLists.txt
+++ b/image_geometry/CMakeLists.txt
@@ -12,7 +12,7 @@ catkin_package(CATKIN_DEPENDS sensor_msgs
 
 catkin_python_setup()
 
-include_directories(SYSTEM ${catkin_INCLUDE_DIRS}
+include_directories(${catkin_INCLUDE_DIRS}
                            ${OpenCV_INCLUDE_DIRS}
 )
 include_directories(include)
diff --git a/image_geometry/package.xml b/image_geometry/package.xml
index f484464..3f554f5 100644
--- a/image_geometry/package.xml
+++ b/image_geometry/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>image_geometry</name>
-  <version>1.11.11</version>
+  <version>1.12.2</version>
   <description>
     `image_geometry` contains C++ and Python libraries for interpreting images
     geometrically. It interfaces the calibration parameters in sensor_msgs/CameraInfo
@@ -18,13 +18,12 @@
 
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libopencv-dev</build_depend>
-  <build_depend>python-opencv</build_depend>
+  <build_depend>opencv3</build_depend>
   <build_depend>sensor_msgs</build_depend>
 
-  <exec_depend>libopencv-dev</exec_depend>
-  <exec_depend>python-opencv</exec_depend>
+  <exec_depend>opencv3</exec_depend>
   <build_export_depend>sensor_msgs</build_export_depend>
 
   <doc_depend>dvipng</doc_depend>
+  <doc_depend>texlive-latex-extra</doc_depend>
 </package>
diff --git a/image_geometry/rosdoc.yaml b/image_geometry/rosdoc.yaml
index 6f9ef15..615dc7e 100644
--- a/image_geometry/rosdoc.yaml
+++ b/image_geometry/rosdoc.yaml
@@ -1,8 +1,8 @@
  - builder: sphinx
    name: Python API
    output_dir: python
+   sphinx_root_dir: doc
  - builder: doxygen
    name: C++ API
    output_dir: c++
    file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
-   sphinx_root_dir: doc
diff --git a/image_geometry/src/image_geometry/cameramodels.py b/image_geometry/src/image_geometry/cameramodels.py
index 04fe15b..139c95c 100644
--- a/image_geometry/src/image_geometry/cameramodels.py
+++ b/image_geometry/src/image_geometry/cameramodels.py
@@ -52,6 +52,8 @@ class PinholeCameraModel:
         self.height = msg.height
         self.binning_x = max(1, msg.binning_x)
         self.binning_y = max(1, msg.binning_y)
+        self.resolution = (msg.width, msg.height)
+
         self.raw_roi = copy.copy(msg.roi)
         # ROI all zeros is considered the same as full resolution
         if (self.raw_roi.x_offset == 0 and self.raw_roi.y_offset == 0 and
@@ -202,6 +204,10 @@ class PinholeCameraModel:
         fy = self.P[1, 1]
         return Z * deltaV / fy
 
+    def fullResolution(self):
+        """Returns the full resolution of the camera"""
+        return self.resolution
+
     def intrinsicMatrix(self):
         """ Returns :math:`K`, also called camera_matrix in cv docs """
         return self.K
diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp
index ea1f940..b0387e9 100644
--- a/image_geometry/src/pinhole_camera_model.cpp
+++ b/image_geometry/src/pinhole_camera_model.cpp
@@ -127,7 +127,16 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
   // Figure out how to handle the distortion
   if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
       cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
-    cache_->distortion_state = (cam_info_.D.size() == 0 || (cam_info_.D[0] == 0.0)) ? NONE : CALIBRATED;
+    // If any distortion coefficient is non-zero, then need to apply the distortion
+    cache_->distortion_state = NONE;
+    for (size_t i = 0; i < cam_info_.D.size(); ++i)
+    {
+      if (cam_info_.D[i] != 0)
+      {
+        cache_->distortion_state = CALIBRATED;
+        break;
+      }
+    }
   }
   else
     cache_->distortion_state = UNKNOWN;
@@ -285,7 +294,13 @@ void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, in
       break;
     case CALIBRATED:
       initRectificationMaps();
-      cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
+      if (raw.depth() == CV_32F || raw.depth() == CV_64F)
+      {
+        cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
+      }
+      else {
+        cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
+      }
       break;
     default:
       assert(cache_->distortion_state == UNKNOWN);
diff --git a/image_geometry/test/utest.cpp b/image_geometry/test/utest.cpp
index ad58896..67e2e4b 100644
--- a/image_geometry/test/utest.cpp
+++ b/image_geometry/test/utest.cpp
@@ -161,6 +161,97 @@ TEST_F(PinholeTest, initialization)
     EXPECT_EQ(camera.projectionMatrix().cols, 4);
 }
 
+TEST_F(PinholeTest, rectifyIfCalibrated)
+{
+  /// @todo use forward distortion for a better test
+  // Ideally this test would have two images stored on disk
+  // one which is distorted and the other which is rectified,
+  // and then rectification would take place here and the output
+  // image compared to the one on disk (which would mean if
+  // the distortion coefficients above can't change once paired with
+  // an image).
+
+  // Later could incorporate distort code
+  // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp)
+  // to take any image distort it, then undistort with rectifyImage,
+  // and given the distortion coefficients are consistent the input image
+  // and final output image should be mostly the same (though some
+  // interpolation error
+  // creeps in), except for outside a masked region where information was lost.
+  // The masked region can be generated with a pure white image that
+  // goes through the same process (if it comes out completely black
+  // then the distortion parameters are problematic).
+
+  // For now generate an image and pass the test simply if
+  // the rectified image does not match the distorted image.
+  // Then zero out the first distortion coefficient and run
+  // the test again.
+  // Then zero out all the distortion coefficients and test
+  // that the output image is the same as the input.
+  cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
+
+  // draw a grid
+  const cv::Scalar color = cv::Scalar(255, 255, 255);
+  // draw the lines thick so the proportion of error due to
+  // interpolation is reduced
+  const int thickness = 7;
+  const int type = 8;
+  for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
+  {
+    cv::line(distorted_image,
+             cv::Point(0, y), cv::Point(cam_info_.width, y),
+             color, type, thickness);
+  }
+  for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
+  {
+    // draw the lines thick so the prorportion of interpolation error is reduced
+    cv::line(distorted_image,
+             cv::Point(x, 0), cv::Point(x, cam_info_.height),
+             color, type, thickness);
+  }
+
+  cv::Mat rectified_image;
+  // Just making this number up, maybe ought to be larger
+  // since a completely different image would be on the order of
+  // width * height * 255 = 78e6
+  const double diff_threshold = 10000.0;
+  double error;
+
+  // Test that rectified image is sufficiently different
+  // using default distortion
+  model_.rectifyImage(distorted_image, rectified_image);
+  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
+  // Just making this number up, maybe ought to be larger
+  EXPECT_GT(error, diff_threshold);
+
+  // Test that rectified image is sufficiently different
+  // using default distortion but with first element zeroed
+  // out.
+  sensor_msgs::CameraInfo cam_info_2 = cam_info_;
+  cam_info_2.D[0] = 0.0;
+  model_.fromCameraInfo(cam_info_2);
+  model_.rectifyImage(distorted_image, rectified_image);
+  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
+  EXPECT_GT(error, diff_threshold);
+
+  // Test that rectified image is the same using zero distortion
+  cam_info_2.D.assign(cam_info_2.D.size(), 0);
+  model_.fromCameraInfo(cam_info_2);
+  model_.rectifyImage(distorted_image, rectified_image);
+  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
+  EXPECT_EQ(error, 0);
+
+  // Test that rectified image is the same using empty distortion
+  cam_info_2.D.clear();
+  model_.fromCameraInfo(cam_info_2);
+  model_.rectifyImage(distorted_image, rectified_image);
+  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
+  EXPECT_EQ(error, 0);
+
+  // restore original distortion
+  model_.fromCameraInfo(cam_info_);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/opencv_apps/CHANGELOG.rst b/opencv_apps/CHANGELOG.rst
deleted file mode 100644
index dda30f2..0000000
--- a/opencv_apps/CHANGELOG.rst
+++ /dev/null
@@ -1,44 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package opencv_apps
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.11.11 (2016-01-31)
---------------------
-* check if opencv_contrib is available
-* Use respawn instead of watch
-* Contributors: Kei Okada, trainman419
-
-1.11.10 (2016-01-16)
---------------------
-* enable simple_flow on opencv3, https://github.com/ros-perception/vision_opencv/commit/8ed5ff5c48b4c3d270cd8216175cf6a8441cb046 can revert https://github.com/ros-perception/vision_opencv/commit/89a933aef7c11acdb75a17c46bfcb60389b25baa
-* lk_flow_nodeletcpp, fback_flow_nodelet.cpp: need to copy input image to gray
-* opencv_apps: add test programs, this will generate images for wiki
-* fix OpenCV3 build
-* phase_corr: fix display, bigger circle and line
-* goodfeature_track_nodelet.cpp: publish good feature points as corners
-* set image encoding to bgr8
-* convex_hull: draw hull with width 4
-* watershed_segmentatoin_nodelet.cpp: output segmented area as contours and suppot add_seed_points as input of segmentatoin seed
-* src/nodelet/segment_objects_nodelet.cpp: change output image topic name from segmented to image
-* Convert rgb image to bgr in lk_flow
-* [oepncv_apps] fix bug for segment_objects_nodelet.cpp
-* Contributors: Kei Okada, Kentaro Wada, Shingo Kitagawa, Vincent Rabaud
-
-1.11.9 (2015-11-29)
--------------------
-* Accept grayscale images as input as well
-* Add format enum for easy use and choose format.
-* Contributors: Felix Mauch, talregev
-
-1.11.8 (2015-07-15)
--------------------
-* simplify the OpenCV3 compatibility
-* fix image output of fback_flow
-* fix error: ISO C++ forbids initialization of member for gcc 4.6
-* add std_srvs
-* add std_srvs
-* fix error: ISO C++ forbids initialization of member for gcc 4.6
-* get opencv_apps to compile with OpenCV3
-* fix licenses for Kei
-* add opencv_apps, proposed in `#40 <https://github.com/ros-perception/vision_opencv/issues/40>`_
-* Contributors: Kei Okada, Vincent Rabaud, Yuto Inagaki
diff --git a/opencv_apps/CMakeLists.txt b/opencv_apps/CMakeLists.txt
deleted file mode 100644
index 79865da..0000000
--- a/opencv_apps/CMakeLists.txt
+++ /dev/null
@@ -1,309 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(opencv_apps)
-
-find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp)
-
-find_package(OpenCV REQUIRED)
-message(STATUS "OpenCV Components: ${OpenCV_LIB_COMPONENTS}")
-if(OpenCV_VERSION VERSION_LESS "3.0" OR TARGET opencv_optflow)
-  set(OPENCV_HAVE_OPTFLOW TRUE)
-endif()
-
-# generate the dynamic_reconfigure config file
-generate_dynamic_reconfigure_options(
-  cfg/EdgeDetection.cfg cfg/HoughLines.cfg cfg/HoughCircles.cfg
-  cfg/FindContours.cfg cfg/ConvexHull.cfg cfg/GeneralContours.cfg cfg/ContourMoments.cfg
-  cfg/FaceDetection.cfg
-  cfg/GoodfeatureTrack.cfg
-  #
-  cfg/CamShift.cfg
-  cfg/FBackFlow.cfg
-  cfg/LKFlow.cfg
-  cfg/PeopleDetect.cfg
-  cfg/PhaseCorr.cfg
-  cfg/SegmentObjects.cfg
-  cfg/SimpleFlow.cfg
-  cfg/WatershedSegmentation.cfg
-  )
-
-## Generate messages in the 'msg' folder
-add_message_files(
-   FILES
-   Point2D.msg
-   Point2DStamped.msg
-   Point2DArray.msg
-   Point2DArrayStamped.msg
-   Rect.msg
-   RectArray.msg
-   RectArrayStamped.msg
-   Flow.msg
-   FlowStamped.msg
-   FlowArray.msg
-   FlowArrayStamped.msg
-   RectArrayStamped.msg
-   Size.msg
-   Face.msg
-   FaceArray.msg
-   FaceArrayStamped.msg
-   Line.msg
-   LineArray.msg
-   LineArrayStamped.msg
-   RotatedRect.msg
-   RotatedRectStamped.msg
-   RotatedRectArray.msg
-   RotatedRectArrayStamped.msg
-   Circle.msg
-   CircleArray.msg
-   CircleArrayStamped.msg
-   Moment.msg
-   MomentArray.msg
-   MomentArrayStamped.msg
-   Contour.msg
-   ContourArray.msg
-   ContourArrayStamped.msg
-)
-
-## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  geometry_msgs
-)
-
-catkin_package()
-
-include_directories( ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
-
-## Declare a cpp library
-if(OPENCV_HAVE_OPTFLOW)
-  list(APPEND ${PROJECT_NAME}_EXTRA_FILES src/nodelet/simple_flow_nodelet.cpp)
-endif()
-
-add_library(${PROJECT_NAME} SHARED
-  # https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code
-  # ImgTrans
-  src/nodelet/edge_detection_nodelet.cpp
-  src/nodelet/hough_lines_nodelet.cpp
-  src/nodelet/hough_circles_nodelet.cpp
-  # ShapeDescriptors
-  src/nodelet/find_contours_nodelet.cpp
-  src/nodelet/convex_hull_nodelet.cpp
-  src/nodelet/general_contours_nodelet.cpp
-  src/nodelet/contour_moments_nodelet.cpp
-  # objectDetection
-  src/nodelet/face_detection_nodelet.cpp
-  # TrackingMotion
-  src/nodelet/goodfeature_track_nodelet.cpp
-  # https://github.com/Itseez/opencv/blob/2.4/samples/cpp
-  # 3calibration.cpp
-  # bagofwords_classification.cpp
-  # bgfg_gmg.cpp
-  # bgfg_segm.cpp
-  # brief_match_test.cpp
-  # build3dmodel.cpp
-  # calibration_artificial.cpp
-  # calibration.cpp
-  src/nodelet/camshift_nodelet.cpp
-  src/nodelet/simple_example_nodelet.cpp
-  src/nodelet/simple_compressed_example_nodelet.cpp
-  # chamfer.cpp
-  # connected_components.cpp
-  # contours2.cpp
-  # convexhull.cpp
-  # cout_mat.cpp
-  # delaunay2.cpp
-  # demhist.cpp
-  # descriptor_extractor_matcher.cpp
-  # detection_based_tracker_sample.cpp
-  # detector_descriptor_evaluation.cpp
-  # detector_descriptor_matcher_evaluation.cpp
-  # dft.cpp
-  # distrans.cpp
-  # drawing.cpp
-  # edge.cpp
-  # em.cpp
-  # fabmap_sample.cpp
-  # facerec_demo.cpp
-  # facial_features.cpp
-  src/nodelet/fback_flow_nodelet.cpp
-  # ffilldemo.cpp
-  # filestorage.cpp
-  # fitellipse.cpp
-  # freak_demo.cpp
-  # gencolors.cpp
-  # generic_descriptor_match.cpp
-  # grabcut.cpp
-  # houghcircles.cpp
-  # houghlines.cpp
-  # hybridtrackingsample.cpp
-  # image.cpp
-  # imagelist_creator.cpp
-  # image_sequence.cpp
-  # inpaint.cpp
-  # intelperc_capture.cpp
-  # kalman.cpp
-  # kmeans.cpp
-  # laplace.cpp
-  # latentsvm_multidetect.cpp
-  # letter_recog.cpp
-  # linemod.cpp
-  src/nodelet/lk_flow_nodelet.cpp
-  # logpolar_bsm.cpp
-  # matcher_simple.cpp
-  # matching_to_many_images.cpp
-  # meanshift_segmentation.cpp
-  # minarea.cpp
-  # morphology2.cpp
-  # opencv_version.cpp
-  # OpenEXRimages_HDR_Retina_toneMapping.cpp
-  # OpenEXRimages_HDR_Retina_toneMapping_video.cpp
-  # openni_capture.cpp
-  # pca.cpp
-  src/nodelet/people_detect_nodelet.cpp
-  src/nodelet/phase_corr_nodelet.cpp
-  # points_classifier.cpp
-  # retinaDemo.cpp
-  # rgbdodometry.cpp
-  src/nodelet/segment_objects_nodelet.cpp
-  ## select3dobj.cpp
-###
-### simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108
-###
-###  src/nodelet/simple_flow_nodelet.cpp
-  # squares.cpp
-  # starter_imagelist.cpp
-  # starter_video.cpp
-  # stereo_calib.cpp
-  # stereo_match.cpp
-  # stitching.cpp
-  # stitching_detailed.cpp
-  # tvl1_optical_flow.cpp
-  ## videocapture_pvapi.cpp
-  ## video_dmtx.cpp
-  # video_homography.cpp
-  # videostab.cpp
-  src/nodelet/watershed_segmentation_nodelet.cpp
-  ${${PROJECT_NAME}_EXTRA_FILES}
-)
-
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages_cpp)
-install(TARGETS ${PROJECT_NAME}
-        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-)
-
-add_executable(edge_detection_exe src/node/edge_detection.cpp)
-SET_TARGET_PROPERTIES(edge_detection_exe PROPERTIES OUTPUT_NAME edge_detection)
-target_link_libraries(edge_detection_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(hough_lines_exe src/node/hough_lines.cpp)
-SET_TARGET_PROPERTIES(hough_lines_exe PROPERTIES OUTPUT_NAME hough_lines)
-target_link_libraries(hough_lines_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(hough_circles_exe src/node/hough_circles.cpp)
-SET_TARGET_PROPERTIES(hough_circles_exe PROPERTIES OUTPUT_NAME hough_circles)
-target_link_libraries(hough_circles_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(find_contours_exe src/node/find_contours.cpp)
-SET_TARGET_PROPERTIES(find_contours_exe PROPERTIES OUTPUT_NAME find_contours)
-target_link_libraries(find_contours_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(convex_hull_exe src/node/convex_hull.cpp)
-SET_TARGET_PROPERTIES(convex_hull_exe PROPERTIES OUTPUT_NAME convex_hull)
-target_link_libraries(convex_hull_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(general_contours_exe src/node/general_contours.cpp)
-SET_TARGET_PROPERTIES(general_contours_exe PROPERTIES OUTPUT_NAME general_contours)
-target_link_libraries(general_contours_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(contour_moments_exe src/node/contour_moments.cpp)
-SET_TARGET_PROPERTIES(contour_moments_exe PROPERTIES OUTPUT_NAME contour_moments)
-target_link_libraries(contour_moments_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(face_detection_exe src/node/face_detection.cpp)
-SET_TARGET_PROPERTIES(face_detection_exe PROPERTIES OUTPUT_NAME face_detection)
-target_link_libraries(face_detection_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(goodfeature_track_exe src/node/goodfeature_track.cpp)
-SET_TARGET_PROPERTIES(goodfeature_track_exe PROPERTIES OUTPUT_NAME goodfeature_track)
-target_link_libraries(goodfeature_track_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-  # bgfg_gmg.cpp
-  # bgfg_segm.cpp
-  # calibration.cpp
-
-add_executable(camshift_exe src/node/camshift.cpp)
-SET_TARGET_PROPERTIES(camshift_exe PROPERTIES OUTPUT_NAME camshift)
-target_link_libraries(camshift_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(simple_example_exe src/node/simple_example.cpp)
-SET_TARGET_PROPERTIES(simple_example_exe PROPERTIES OUTPUT_NAME simple_example)
-target_link_libraries(simple_example_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(simple_compressed_example_exe src/node/simple_compressed_example.cpp)
-SET_TARGET_PROPERTIES(simple_compressed_example_exe PROPERTIES OUTPUT_NAME simple_compressed_example)
-target_link_libraries(simple_compressed_example_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(fback_flow_exe src/node/fback_flow.cpp)
-SET_TARGET_PROPERTIES(fback_flow_exe PROPERTIES OUTPUT_NAME fback_flow)
-target_link_libraries(fback_flow_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-  # fback.cpp
-  # hybridtrackingsample.cpp
-  # image_sequence.cpp
-  # intelperc_capture.cpp
-  # laplace.cpp
-  # linemod.cpp
-  # lkdemo.cpp
-
-add_executable(lk_flow_exe src/node/lk_flow.cpp)
-SET_TARGET_PROPERTIES(lk_flow_exe PROPERTIES OUTPUT_NAME lk_flow)
-target_link_libraries(lk_flow_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(people_detect_exe src/node/people_detect.cpp)
-SET_TARGET_PROPERTIES(people_detect_exe PROPERTIES OUTPUT_NAME people_detect)
-target_link_libraries(people_detect_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-add_executable(phase_corr_exe src/node/phase_corr.cpp)
-SET_TARGET_PROPERTIES(phase_corr_exe PROPERTIES OUTPUT_NAME phase_corr)
-target_link_libraries(phase_corr_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-  # phase_corr.cpp
-  # retinaDemo.cpp
-  # segment_objects.cpp
-
-add_executable(segment_objects_exe src/node/segment_objects.cpp)
-SET_TARGET_PROPERTIES(segment_objects_exe PROPERTIES OUTPUT_NAME segment_objects)
-target_link_libraries(segment_objects_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-  # select3dobj.cpp
-
-# simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108
-if(OPENCV_HAVE_OPTFLOW)
-  add_executable(simple_flow_exe src/node/simple_flow.cpp)
-  SET_TARGET_PROPERTIES(simple_flow_exe PROPERTIES OUTPUT_NAME simple_flow)
-  target_link_libraries(simple_flow_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-endif()
-
-  # starter_video.cpp
-  # videocapture_pvapi.cpp
-  # video_dmtx.cpp
-  # video_homography.cpp
-
-add_executable(watershed_segmentation_exe src/node/watershed_segmentation.cpp)
-SET_TARGET_PROPERTIES(watershed_segmentation_exe PROPERTIES OUTPUT_NAME watershed_segmentation)
-target_link_libraries(watershed_segmentation_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
-
-
-install(TARGETS edge_detection_exe hough_lines_exe hough_circles_exe face_detection_exe goodfeature_track_exe
-  camshift_exe simple_example_exe simple_compressed_example_exe fback_flow_exe lk_flow_exe people_detect_exe phase_corr_exe segment_objects_exe watershed_segmentation_exe
-        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-install(FILES nodelet_plugins.xml
-        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-## test
-if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS rostest)
-  add_subdirectory(test)
-endif()
diff --git a/opencv_apps/cfg/CamShift.cfg b/opencv_apps/cfg/CamShift.cfg
deleted file mode 100755
index 1a66a56..0000000
--- a/opencv_apps/cfg/CamShift.cfg
+++ /dev/null
@@ -1,46 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='camshift'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("vmin", int_t, 0, "Vmin",  10, 1, 256)
-gen.add("vmax", int_t, 0, "Vmax", 256, 1, 256)
-gen.add("smin", int_t, 0, "Smin",  30, 1, 256)
-
-exit(gen.generate(PACKAGE, "camshift", "CamShift"))
diff --git a/opencv_apps/cfg/ContourMoments.cfg b/opencv_apps/cfg/ContourMoments.cfg
deleted file mode 100755
index 5fa9f0d..0000000
--- a/opencv_apps/cfg/ContourMoments.cfg
+++ /dev/null
@@ -1,45 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='contour_moments'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("canny_low_threshold", int_t, 0, "Canny Edge low Threshold", 100, 1, 255)
-
-exit(gen.generate(PACKAGE, "contour_moments", "ContourMoments"))
-
diff --git a/opencv_apps/cfg/ConvexHull.cfg b/opencv_apps/cfg/ConvexHull.cfg
deleted file mode 100755
index 7526c30..0000000
--- a/opencv_apps/cfg/ConvexHull.cfg
+++ /dev/null
@@ -1,45 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='convex_hull'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("threshold", int_t, 0, "Detect edges using Threshold", 100, 1, 255)
-
-exit(gen.generate(PACKAGE, "convex_hull", "ConvexHull"))
-
diff --git a/opencv_apps/cfg/EdgeDetection.cfg b/opencv_apps/cfg/EdgeDetection.cfg
deleted file mode 100755
index 8d1ae69..0000000
--- a/opencv_apps/cfg/EdgeDetection.cfg
+++ /dev/null
@@ -1,50 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='edge_detection'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-edge_type = gen.enum([ gen.const("Sobel",   int_t,  0, "Sobel Derivatives"),
-                       gen.const("Laplace", int_t,  1, "Laplace Operator"),
-                       gen.const("Canny",   int_t,  2, "Canny Edge Detector")], "An enum for Edge Detection Mehtods")
-gen.add("edge_type", int_t, 0, "Edge Detection Methods", 0, 0, 2, edit_method=edge_type)
-
-gen.add("canny_low_threshold", int_t, 0, "Canny Edge low Threshold", 10, 1, 100)
-
-
-exit(gen.generate(PACKAGE, "edge_detection", "EdgeDetection"))
diff --git a/opencv_apps/cfg/FBackFlow.cfg b/opencv_apps/cfg/FBackFlow.cfg
deleted file mode 100755
index 61217a4..0000000
--- a/opencv_apps/cfg/FBackFlow.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='fback_flow'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "fback_flow", "FBackFlow"))
diff --git a/opencv_apps/cfg/FaceDetection.cfg b/opencv_apps/cfg/FaceDetection.cfg
deleted file mode 100755
index 09e83f5..0000000
--- a/opencv_apps/cfg/FaceDetection.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='face_detection'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "face_detection", "FaceDetection"))
diff --git a/opencv_apps/cfg/FindContours.cfg b/opencv_apps/cfg/FindContours.cfg
deleted file mode 100755
index 7c8346e..0000000
--- a/opencv_apps/cfg/FindContours.cfg
+++ /dev/null
@@ -1,45 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='find_contours'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("canny_low_threshold", int_t, 0, "Canny Edge low Threshold", 10, 1, 255)
-
-exit(gen.generate(PACKAGE, "find_contours", "FindContours"))
-
diff --git a/opencv_apps/cfg/GeneralContours.cfg b/opencv_apps/cfg/GeneralContours.cfg
deleted file mode 100755
index bda0be2..0000000
--- a/opencv_apps/cfg/GeneralContours.cfg
+++ /dev/null
@@ -1,45 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='general_contours'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("threshold", int_t, 0, "Detect edges using Threshold", 100, 1, 255)
-
-exit(gen.generate(PACKAGE, "general_contours", "GeneralContours"))
-
diff --git a/opencv_apps/cfg/GoodfeatureTrack.cfg b/opencv_apps/cfg/GoodfeatureTrack.cfg
deleted file mode 100755
index 60ed879..0000000
--- a/opencv_apps/cfg/GoodfeatureTrack.cfg
+++ /dev/null
@@ -1,44 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='goodfeature_track'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("max_corners", int_t, 0, "Max Number of Corners", 23, 1, 100)
-
-exit(gen.generate(PACKAGE, "goodfeature_track", "GoodfeatureTrack"))
diff --git a/opencv_apps/cfg/HoughCircles.cfg b/opencv_apps/cfg/HoughCircles.cfg
deleted file mode 100755
index 46dbd7d..0000000
--- a/opencv_apps/cfg/HoughCircles.cfg
+++ /dev/null
@@ -1,45 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='hough_circles'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("canny_threshold", int_t, 0, "Canny threshold", 200, 1, 255)
-gen.add("accumulator_threshold", int_t, 0, "Accumulator threshold", 50, 1, 200)
-
-exit(gen.generate(PACKAGE, "hough_circles", "HoughCircles"))
diff --git a/opencv_apps/cfg/HoughLines.cfg b/opencv_apps/cfg/HoughLines.cfg
deleted file mode 100755
index 2d234be..0000000
--- a/opencv_apps/cfg/HoughLines.cfg
+++ /dev/null
@@ -1,48 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='hough_lines'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-hough_type = gen.enum([ gen.const("Standard_Hough_Transform",   int_t,  0, "Standard Hough Line"),
-                        gen.const("Probabilistic_Hough_Transform",   int_t,  1, "Probabilistic Hough Line")], "An enum for Hough Transform Mehtods")
-gen.add("hough_type", int_t, 0, "Hough Line Methods", 0, 0, 1, edit_method=hough_type)
-gen.add("threshold", int_t, 150, "Hough Line Threshold", 150, 50, 150)
-
-
-exit(gen.generate(PACKAGE, "hough_lines", "HoughLines"))
diff --git a/opencv_apps/cfg/LKFlow.cfg b/opencv_apps/cfg/LKFlow.cfg
deleted file mode 100755
index ca1b738..0000000
--- a/opencv_apps/cfg/LKFlow.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='lk_flow'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "lk_flow", "LKFlow"))
diff --git a/opencv_apps/cfg/PeopleDetect.cfg b/opencv_apps/cfg/PeopleDetect.cfg
deleted file mode 100755
index 5f12a0c..0000000
--- a/opencv_apps/cfg/PeopleDetect.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='people_detect'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "people_detect", "PeopleDetect"))
diff --git a/opencv_apps/cfg/PhaseCorr.cfg b/opencv_apps/cfg/PhaseCorr.cfg
deleted file mode 100755
index b0b446b..0000000
--- a/opencv_apps/cfg/PhaseCorr.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='phase_corr'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "phase_corr", "PhaseCorr"))
diff --git a/opencv_apps/cfg/SegmentObjects.cfg b/opencv_apps/cfg/SegmentObjects.cfg
deleted file mode 100755
index f12aea1..0000000
--- a/opencv_apps/cfg/SegmentObjects.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='segment_objects'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "segment_objects", "SegmentObjects"))
diff --git a/opencv_apps/cfg/SimpleFlow.cfg b/opencv_apps/cfg/SimpleFlow.cfg
deleted file mode 100755
index b540324..0000000
--- a/opencv_apps/cfg/SimpleFlow.cfg
+++ /dev/null
@@ -1,44 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='simple_flow'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-gen.add("scale", int_t, 0, "Scale", 4, 1, 24)
-
-exit(gen.generate(PACKAGE, "simple_flow", "SimpleFlow"))
diff --git a/opencv_apps/cfg/WatershedSegmentation.cfg b/opencv_apps/cfg/WatershedSegmentation.cfg
deleted file mode 100755
index d4541e5..0000000
--- a/opencv_apps/cfg/WatershedSegmentation.cfg
+++ /dev/null
@@ -1,42 +0,0 @@
-#! /usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2014, Kei Okada.
-# 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 Kei Okada 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.
-
-
-PACKAGE='watershed_segmentation'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
-
-exit(gen.generate(PACKAGE, "watershed_segmentation", "WatershedSegmentation"))
diff --git a/opencv_apps/msg/Circle.msg b/opencv_apps/msg/Circle.msg
deleted file mode 100644
index 08a1c03..0000000
--- a/opencv_apps/msg/Circle.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Point2D center
-float64 radius
-
diff --git a/opencv_apps/msg/CircleArray.msg b/opencv_apps/msg/CircleArray.msg
deleted file mode 100644
index 0de86cc..0000000
--- a/opencv_apps/msg/CircleArray.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Circle[] circles
-
diff --git a/opencv_apps/msg/CircleArrayStamped.msg b/opencv_apps/msg/CircleArrayStamped.msg
deleted file mode 100644
index a330234..0000000
--- a/opencv_apps/msg/CircleArrayStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-Circle[] circles
-
diff --git a/opencv_apps/msg/Contour.msg b/opencv_apps/msg/Contour.msg
deleted file mode 100644
index b5127a0..0000000
--- a/opencv_apps/msg/Contour.msg
+++ /dev/null
@@ -1 +0,0 @@
-Point2D[] points
diff --git a/opencv_apps/msg/ContourArray.msg b/opencv_apps/msg/ContourArray.msg
deleted file mode 100644
index 3ea78c4..0000000
--- a/opencv_apps/msg/ContourArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Contour[] contours
diff --git a/opencv_apps/msg/ContourArrayStamped.msg b/opencv_apps/msg/ContourArrayStamped.msg
deleted file mode 100644
index f9479f6..0000000
--- a/opencv_apps/msg/ContourArrayStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-Contour[] contours
-
diff --git a/opencv_apps/msg/Face.msg b/opencv_apps/msg/Face.msg
deleted file mode 100644
index 557556b..0000000
--- a/opencv_apps/msg/Face.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Rect face
-Rect[] eyes
diff --git a/opencv_apps/msg/FaceArray.msg b/opencv_apps/msg/FaceArray.msg
deleted file mode 100644
index 89bdb0a..0000000
--- a/opencv_apps/msg/FaceArray.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Face[] faces
-
diff --git a/opencv_apps/msg/FaceArrayStamped.msg b/opencv_apps/msg/FaceArrayStamped.msg
deleted file mode 100644
index 4439682..0000000
--- a/opencv_apps/msg/FaceArrayStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-Face[] faces
-
diff --git a/opencv_apps/msg/Flow.msg b/opencv_apps/msg/Flow.msg
deleted file mode 100644
index e478513..0000000
--- a/opencv_apps/msg/Flow.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Point2D point
-Point2D velocity
diff --git a/opencv_apps/msg/FlowArray.msg b/opencv_apps/msg/FlowArray.msg
deleted file mode 100644
index 68685c0..0000000
--- a/opencv_apps/msg/FlowArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Flow[] flow
diff --git a/opencv_apps/msg/FlowArrayStamped.msg b/opencv_apps/msg/FlowArrayStamped.msg
deleted file mode 100644
index b7f2eaf..0000000
--- a/opencv_apps/msg/FlowArrayStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-Flow[] flow
diff --git a/opencv_apps/msg/FlowStamped.msg b/opencv_apps/msg/FlowStamped.msg
deleted file mode 100644
index 1467646..0000000
--- a/opencv_apps/msg/FlowStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-Flow flow
diff --git a/opencv_apps/msg/Line.msg b/opencv_apps/msg/Line.msg
deleted file mode 100644
index 1083a9b..0000000
--- a/opencv_apps/msg/Line.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Point2D pt1
-Point2D pt2
-
diff --git a/opencv_apps/msg/LineArray.msg b/opencv_apps/msg/LineArray.msg
deleted file mode 100644
index f0e8de6..0000000
--- a/opencv_apps/msg/LineArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Line[] lines
diff --git a/opencv_apps/msg/LineArrayStamped.msg b/opencv_apps/msg/LineArrayStamped.msg
deleted file mode 100644
index d72ff61..0000000
--- a/opencv_apps/msg/LineArrayStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-Line[] lines
diff --git a/opencv_apps/msg/Moment.msg b/opencv_apps/msg/Moment.msg
deleted file mode 100644
index eca8ba4..0000000
--- a/opencv_apps/msg/Moment.msg
+++ /dev/null
@@ -1,34 +0,0 @@
-# spatial moments
-float64 m00
-float64 m10
-float64 m01
-float64 m20
-float64 m11
-float64 m02
-float64 m30
-float64 m21
-float64 m12
-float64 m03
-
-# central moments
-float64 mu20
-float64 mu11
-float64 mu02
-float64 mu30
-float64 mu21
-float64 mu12
-float64 mu03
-
-# central normalized moments
-float64 nu20
-float64 nu11
-float64 nu02
-float64 nu30
-float64 nu21
-float64 nu12
-float64 nu03
-
-# center of mass m10/m00, m01/m00
-Point2D center
-float64 length
-float64 area
diff --git a/opencv_apps/msg/MomentArray.msg b/opencv_apps/msg/MomentArray.msg
deleted file mode 100644
index b02c22c..0000000
--- a/opencv_apps/msg/MomentArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Moment[] moments
diff --git a/opencv_apps/msg/MomentArrayStamped.msg b/opencv_apps/msg/MomentArrayStamped.msg
deleted file mode 100644
index 0b6b6b9..0000000
--- a/opencv_apps/msg/MomentArrayStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-Moment[] moments
diff --git a/opencv_apps/msg/Point2D.msg b/opencv_apps/msg/Point2D.msg
deleted file mode 100644
index ed7a246..0000000
--- a/opencv_apps/msg/Point2D.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-float64 x
-float64 y
-
diff --git a/opencv_apps/msg/Point2DArray.msg b/opencv_apps/msg/Point2DArray.msg
deleted file mode 100644
index b5127a0..0000000
--- a/opencv_apps/msg/Point2DArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Point2D[] points
diff --git a/opencv_apps/msg/Point2DArrayStamped.msg b/opencv_apps/msg/Point2DArrayStamped.msg
deleted file mode 100644
index e52a8c5..0000000
--- a/opencv_apps/msg/Point2DArrayStamped.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-Header header
-Point2D[] points
diff --git a/opencv_apps/msg/Point2DStamped.msg b/opencv_apps/msg/Point2DStamped.msg
deleted file mode 100644
index 89f3a0e..0000000
--- a/opencv_apps/msg/Point2DStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-Point2D point
-
diff --git a/opencv_apps/msg/Rect.msg b/opencv_apps/msg/Rect.msg
deleted file mode 100644
index ea99932..0000000
--- a/opencv_apps/msg/Rect.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# opencv Rect data type, x-y is center point
-float64 x
-float64 y
-float64 width
-float64 height
-
diff --git a/opencv_apps/msg/RectArray.msg b/opencv_apps/msg/RectArray.msg
deleted file mode 100644
index 6a8e226..0000000
--- a/opencv_apps/msg/RectArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-Rect[] rects
diff --git a/opencv_apps/msg/RectArrayStamped.msg b/opencv_apps/msg/RectArrayStamped.msg
deleted file mode 100644
index e238d66..0000000
--- a/opencv_apps/msg/RectArrayStamped.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-Header header
-Rect[] rects
-
-
diff --git a/opencv_apps/msg/RotatedRect.msg b/opencv_apps/msg/RotatedRect.msg
deleted file mode 100644
index 4b6086c..0000000
--- a/opencv_apps/msg/RotatedRect.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-float64 angle
-Point2D center
-Size size
diff --git a/opencv_apps/msg/RotatedRectArray.msg b/opencv_apps/msg/RotatedRectArray.msg
deleted file mode 100644
index ac35278..0000000
--- a/opencv_apps/msg/RotatedRectArray.msg
+++ /dev/null
@@ -1 +0,0 @@
-RotatedRect[] rects
diff --git a/opencv_apps/msg/RotatedRectArrayStamped.msg b/opencv_apps/msg/RotatedRectArrayStamped.msg
deleted file mode 100644
index b10aa7d..0000000
--- a/opencv_apps/msg/RotatedRectArrayStamped.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-Header header
-RotatedRect[] rects
-
diff --git a/opencv_apps/msg/RotatedRectStamped.msg b/opencv_apps/msg/RotatedRectStamped.msg
deleted file mode 100644
index 4c03dda..0000000
--- a/opencv_apps/msg/RotatedRectStamped.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-Header header
-RotatedRect rect
-
-
diff --git a/opencv_apps/msg/Size.msg b/opencv_apps/msg/Size.msg
deleted file mode 100644
index efcc948..0000000
--- a/opencv_apps/msg/Size.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-float64 width
-float64 height
-
diff --git a/opencv_apps/nodelet_plugins.xml b/opencv_apps/nodelet_plugins.xml
deleted file mode 100644
index 56203d6..0000000
--- a/opencv_apps/nodelet_plugins.xml
+++ /dev/null
@@ -1,79 +0,0 @@
-<library path="lib/libopencv_apps">
-
-  <class name="edge_detection/edge_detection" type="edge_detection::EdgeDetectionNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find edges</description>
-  </class>
-
-  <class name="hough_lines/hough_lines" type="hough_lines::HoughLinesNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find lines</description>
-  </class>
-
-  <class name="hough_circles/hough_circles" type="hough_circles::HoughCirclesNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find circles</description>
-  </class>
-
-  <class name="find_contours/find_contours" type="find_contours::FindContoursNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find contours</description>
-  </class>
-
-  <class name="convex_hull/convex_hull" type="convex_hull::ConvexHullNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find convex hulls</description>
-  </class>
-
-  <class name="general_contours/general_contours" type="general_contours::GeneralContoursNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to creating bounding boxes and circles for contours</description>
-  </class>
-
-  <class name="contour_moments/contour_moments" type="contour_moments::ContourMomentsNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find image moments</description>
-  </class>
-
-  <class name="face_detection/face_detection" type="face_detection::FaceDetectionNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to find faces</description>
-  </class>
-
-  <class name="goodfeature_track/goodfeature_track" type="goodfeature_track::GoodfeatureTrackNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet for detecting corners using Shi-Tomasi method</description>
-  </class>
-
-  <class name="camshift/camshift" type="camshift::CamShiftNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to show mean-shift based tracking</description>
-  </class>
-
-  <class name="fback_flow/fback_flow" type="fback_flow::FBackFlowNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to demonstrates dense optical flow algorithm by Gunnar Farneback</description>
-  </class>
-
-  <class name="lk_flow/lk_flow" type="lk_flow::LKFlowNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to calculate Lukas-Kanade optical flow</description>
-  </class>
-
-  <class name="people_detect/people_detect" type="people_detect::PeopleDetectNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to demonstrate the use of the HoG descriptor</description>
-  </class>
-
-  <class name="phase_corr/phase_corr" type="phase_corr::PhaseCorrNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to demonstrate the use of phaseCorrelate</description>
-  </class>
-
-  <class name="segment_objects/segment_objects" type="segment_objects::SegmentObjectsNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to demonstrate a simple method of connected components clean up of background subtraction</description>
-  </class>
-
-  <class name="simple_flow/simple_flow" type="simple_flow::SimpleFlowNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet of SimpleFlow optical flow algorithm</description>
-  </class>
-  
- <class name="simple_example/simple_example" type="simple_example::SimpleExampleNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet of Simple Example from wiki</description>
-  </class>
-  
-   <class name="simple_compressed_example/simple_compressed_example" type="simple_compressed_example::SimpleCompressedExampleNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet of Simple Example from wiki</description>
-  </class>
-
-  <class name="watershed_segmentation/watershed_segmentation" type="watershed_segmentation::WatershedSegmentationNodelet" base_class_type="nodelet::Nodelet">
-    <description>Nodelet to demonstrate the famous watershed segmentation algorithm in OpenCV: watershed()</description>
-  </class>
-
-</library>
diff --git a/opencv_apps/package.xml b/opencv_apps/package.xml
deleted file mode 100644
index 5d48b1e..0000000
--- a/opencv_apps/package.xml
+++ /dev/null
@@ -1,41 +0,0 @@
-<?xml version="1.0"?>
-<package>
-  <name>opencv_apps</name>
-  <version>1.11.11</version>
-  <description>The opencv_apps package, most of code is taken from https://github.com/Itseez/opencv/tree/master/samples/cpp</description>
-
-  <maintainer email="kei.okada at gmail.com">Kei Okada</maintainer>
-  <author email="kei.okada at gmail.com">Kei Okada</author>
-
-  <license>BSD</license>
-
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>dynamic_reconfigure</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>message_generation</build_depend>
-  <build_depend>nodelet</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>std_srvs</build_depend>
-
-  <run_depend>cv_bridge</run_depend>
-  <run_depend>dynamic_reconfigure</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>message_runtime</run_depend>
-  <run_depend>nodelet</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>std_srvs</run_depend>
-
-  <test_depend>rostest</test_depend>
-  <test_depend>rosbag</test_depend>
-  <test_depend>rosservice</test_depend>
-  <test_depend>rostopic</test_depend>
-  <test_depend>image_proc</test_depend>
-  <test_depend>image_view</test_depend>
-  <test_depend>topic_tools</test_depend>
-
-  <export>
-    <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
-  </export>
-</package>
diff --git a/opencv_apps/src/node/camshift.cpp b/opencv_apps/src/node/camshift.cpp
deleted file mode 100644
index dd3975d..0000000
--- a/opencv_apps/src/node/camshift.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "camshift", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "camshift/camshift", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/contour_moments.cpp b/opencv_apps/src/node/contour_moments.cpp
deleted file mode 100644
index 3a6c466..0000000
--- a/opencv_apps/src/node/contour_moments.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "contour_moments", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "contour_moments/contour_moments", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
-
diff --git a/opencv_apps/src/node/convex_hull.cpp b/opencv_apps/src/node/convex_hull.cpp
deleted file mode 100644
index 84b3fd2..0000000
--- a/opencv_apps/src/node/convex_hull.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "convex_hull", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "convex_hull/convex_hull", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
-
diff --git a/opencv_apps/src/node/edge_detection.cpp b/opencv_apps/src/node/edge_detection.cpp
deleted file mode 100644
index 15e7475..0000000
--- a/opencv_apps/src/node/edge_detection.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "edge_detection", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "edge_detection/edge_detection", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
-
diff --git a/opencv_apps/src/node/face_detection.cpp b/opencv_apps/src/node/face_detection.cpp
deleted file mode 100644
index fcc14e3..0000000
--- a/opencv_apps/src/node/face_detection.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "face_detection", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "face_detection/face_detection", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/fback_flow.cpp b/opencv_apps/src/node/fback_flow.cpp
deleted file mode 100644
index c6cf50e..0000000
--- a/opencv_apps/src/node/fback_flow.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "fback_flow", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "fback_flow/fback_flow", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/find_contours.cpp b/opencv_apps/src/node/find_contours.cpp
deleted file mode 100644
index c4cc847..0000000
--- a/opencv_apps/src/node/find_contours.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "find_contours", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "find_contours/find_contours", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
-
diff --git a/opencv_apps/src/node/general_contours.cpp b/opencv_apps/src/node/general_contours.cpp
deleted file mode 100644
index fb01385..0000000
--- a/opencv_apps/src/node/general_contours.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "general_contours", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "general_contours/general_contours", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
-
diff --git a/opencv_apps/src/node/goodfeature_track.cpp b/opencv_apps/src/node/goodfeature_track.cpp
deleted file mode 100644
index 389fdac..0000000
--- a/opencv_apps/src/node/goodfeature_track.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "goodfeature_track", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "goodfeature_track/goodfeature_track", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/hough_circles.cpp b/opencv_apps/src/node/hough_circles.cpp
deleted file mode 100644
index 068a7dd..0000000
--- a/opencv_apps/src/node/hough_circles.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "hough_circles", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "hough_circles/hough_circles", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/hough_lines.cpp b/opencv_apps/src/node/hough_lines.cpp
deleted file mode 100644
index c6e8ecb..0000000
--- a/opencv_apps/src/node/hough_lines.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "hough_lines", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "hough_lines/hough_lines", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/lk_flow.cpp b/opencv_apps/src/node/lk_flow.cpp
deleted file mode 100644
index 11c1978..0000000
--- a/opencv_apps/src/node/lk_flow.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "lk_flow", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "lk_flow/lk_flow", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/people_detect.cpp b/opencv_apps/src/node/people_detect.cpp
deleted file mode 100644
index 5ecc344..0000000
--- a/opencv_apps/src/node/people_detect.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "people_detect", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "people_detect/people_detect", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/phase_corr.cpp b/opencv_apps/src/node/phase_corr.cpp
deleted file mode 100644
index d7949f4..0000000
--- a/opencv_apps/src/node/phase_corr.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "phase_corr", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "phase_corr/phase_corr", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/segment_objects.cpp b/opencv_apps/src/node/segment_objects.cpp
deleted file mode 100644
index 08d939e..0000000
--- a/opencv_apps/src/node/segment_objects.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "segment_objects", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "segment_objects/segment_objects", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/simple_compressed_example.cpp b/opencv_apps/src/node/simple_compressed_example.cpp
deleted file mode 100644
index 417001d..0000000
--- a/opencv_apps/src/node/simple_compressed_example.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2015, Tal Regev.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "simple_compressed_example", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "simple_compressed_example/simple_compressed_example", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/simple_example.cpp b/opencv_apps/src/node/simple_example.cpp
deleted file mode 100644
index a483c40..0000000
--- a/opencv_apps/src/node/simple_example.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2015, Tal Regev.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "simple_example", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "simple_example/simple_example", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/simple_flow.cpp b/opencv_apps/src/node/simple_flow.cpp
deleted file mode 100644
index 15cc3f9..0000000
--- a/opencv_apps/src/node/simple_flow.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "simple_flow", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "simple_flow/simple_flow", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/node/watershed_segmentation.cpp b/opencv_apps/src/node/watershed_segmentation.cpp
deleted file mode 100644
index d6b96c3..0000000
--- a/opencv_apps/src/node/watershed_segmentation.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-#include <ros/ros.h>
-#include <nodelet/loader.h>
-
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "watershed_segmentation", ros::init_options::AnonymousName);
-    if (ros::names::remap("image") == "image") {
-        ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
-                 "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
-    }
-
-    nodelet::Loader manager(false);
-    nodelet::M_string remappings;
-    nodelet::V_string my_argv(argv + 1, argv + argc);
-    my_argv.push_back("--shutdown-on-close"); // Internal
-
-    manager.load(ros::this_node::getName(), "watershed_segmentation/watershed_segmentation", remappings, my_argv);
-
-    ros::spin();
-    return 0;
-}
diff --git a/opencv_apps/src/nodelet/camshift_nodelet.cpp b/opencv_apps/src/nodelet/camshift_nodelet.cpp
deleted file mode 100644
index 8c2f6e3..0000000
--- a/opencv_apps/src/nodelet/camshift_nodelet.cpp
+++ /dev/null
@@ -1,472 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/camshiftdemo.cpp
-/**
- * This is a demo that shows mean-shift based tracking
- * You select a color objects such as your face and it tracks it.
- * This reads from video camera (0 by default, or the camera number the user enters
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <iostream>
-#include <ctype.h>
-#include <opencv2/video/tracking.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/CamShiftConfig.h"
-#include "opencv_apps/RotatedRectStamped.h"
-
-namespace camshift {
-class CamShiftNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_, bproj_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  camshift::CamShiftConfig config_;
-  dynamic_reconfigure::Server<camshift::CamShiftConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_, histogram_name_;
-  static bool need_config_update_;
-  static bool on_mouse_update_;
-  static int on_mouse_event_;
-  static int on_mouse_x_;
-  static int on_mouse_y_;
-
-  int vmin_, vmax_, smin_;
-  bool backprojMode;
-  bool selectObject;
-  int trackObject;
-  bool showHist;
-  cv::Point origin;
-  cv::Rect selection;
-  bool paused;
-
-  cv::Rect trackWindow;
-  int hsize;
-  float hranges[2];
-  const float* phranges;
-  cv::Mat hist, histimg;
-  //cv::Mat hsv;
-  
-  static void onMouse( int event, int x, int y, int, void* )
-  {
-    on_mouse_update_ = true;
-    on_mouse_event_ = event;
-    on_mouse_x_ = x;
-    on_mouse_y_ = y;
-  }
-
-  void reconfigureCallback(camshift::CamShiftConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    vmin_ = config_.vmin;
-    vmax_ = config_.vmax;
-    smin_ = config_.smin;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-      cv::Mat backproj;
-      
-      // Messages
-      opencv_apps::RotatedRectStamped rect_msg;
-      rect_msg.header = msg->header;
-
-      // Do the work
-      
-      if( debug_view_) {
-        /// Create Trackbars for Thresholds
-
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-
-        cv::setMouseCallback( window_name_, onMouse, 0 );
-        cv::createTrackbar( "Vmin", window_name_, &vmin_, 256, trackbarCallback);
-        cv::createTrackbar( "Vmax", window_name_, &vmax_, 256, trackbarCallback);
-        cv::createTrackbar( "Smin", window_name_, &smin_, 256, trackbarCallback);
-
-        if (need_config_update_) {
-          config_.vmin = vmin_;
-          config_.vmax = vmax_;
-          config_.smin = smin_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      if ( on_mouse_update_ ) {
-        int event = on_mouse_event_;
-        int x = on_mouse_x_;
-        int y = on_mouse_y_;
-
-        if( selectObject )
-        {
-          selection.x = MIN(x, origin.x);
-          selection.y = MIN(y, origin.y);
-          selection.width = std::abs(x - origin.x);
-          selection.height = std::abs(y - origin.y);
-
-          selection &= cv::Rect(0, 0, frame.cols, frame.rows);
-        }
-
-        switch( event )
-        {
-          case cv::EVENT_LBUTTONDOWN:
-            origin = cv::Point(x,y);
-            selection = cv::Rect(x,y,0,0);
-            selectObject = true;
-            break;
-          case cv::EVENT_LBUTTONUP:
-            selectObject = false;
-            if( selection.width > 0 && selection.height > 0 )
-              trackObject = -1;
-            break;
-        }
-        on_mouse_update_ = false;
-      }
-
-      if( !paused )
-      {
-        cv::Mat hsv, hue, mask;
-        cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV);
-
-        if( trackObject )
-        {
-          int _vmin = vmin_, _vmax = vmax_;
-
-          cv::inRange(hsv, cv::Scalar(0, smin_, MIN(_vmin,_vmax)),
-                      cv::Scalar(180, 256, MAX(_vmin, _vmax)), mask);
-          int ch[] = {0, 0};
-          hue.create(hsv.size(), hsv.depth());
-          cv::mixChannels(&hsv, 1, &hue, 1, ch, 1);
-
-          if( trackObject < 0 )
-          {
-            cv::Mat roi(hue, selection), maskroi(mask, selection);
-            cv::calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
-            cv::normalize(hist, hist, 0, 255, cv::NORM_MINMAX);
-            std::vector<float> hist_value;
-            hist_value.resize(hsize);
-            for(int i = 0; i < hsize; i ++) { hist_value[i] = hist.at<float>(i);}
-            local_nh_.setParam("histogram", hist_value);
-
-            trackWindow = selection;
-            trackObject = 1;
-
-            histimg = cv::Scalar::all(0);
-            int binW = histimg.cols / hsize;
-            cv::Mat buf(1, hsize, CV_8UC3);
-            for( int i = 0; i < hsize; i++ )
-              buf.at<cv::Vec3b>(i) = cv::Vec3b(cv::saturate_cast<uchar>(i*180./hsize), 255, 255);
-            cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR);
-
-            for( int i = 0; i < hsize; i++ )
-            {
-              int val = cv::saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
-              cv::rectangle( histimg, cv::Point(i*binW,histimg.rows),
-                             cv::Point((i+1)*binW,histimg.rows - val),
-                             cv::Scalar(buf.at<cv::Vec3b>(i)), -1, 8 );
-            }
-          }
-
-          cv::calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
-          backproj &= mask;
-          cv::RotatedRect trackBox = cv::CamShift(backproj, trackWindow,
-                                                  cv::TermCriteria( cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 10, 1 ));
-          if( trackWindow.area() <= 1 )
-          {
-            int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
-            //trackWindow = cv::Rect(trackWindow.x - r, trackWindow.y - r,
-            //                       trackWindow.x + r, trackWindow.y + r) &
-            trackWindow = cv::Rect(cols/2 - r, rows/2 - r,
-                                   cols/2 + r, rows/2 + r) &
-              cv::Rect(0, 0, cols, rows);
-          }
-
-          if( backprojMode )
-            cv::cvtColor( backproj, frame, cv::COLOR_GRAY2BGR );
-#ifndef CV_VERSION_EPOCH
-          cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, cv::LINE_AA );
-#else
-          cv::ellipse( frame, trackBox, cv::Scalar(0,0,255), 3, CV_AA );
-#endif
-          
-          rect_msg.rect.angle = trackBox.angle;
-          opencv_apps::Point2D point_msg;
-          opencv_apps::Size size_msg;
-          point_msg.x = trackBox.center.x;
-          point_msg.y = trackBox.center.y;
-          size_msg.width = trackBox.size.width;
-          size_msg.height = trackBox.size.height;
-          rect_msg.rect.center = point_msg;
-          rect_msg.rect.size = size_msg;
-        }
-      }
-      else if( trackObject < 0 )
-        paused = false;
-
-      if( selectObject && selection.width > 0 && selection.height > 0 )
-      {
-        cv::Mat roi(frame, selection);
-        bitwise_not(roi, roi);
-      }
-
-      if( debug_view_ ) {
-        cv::imshow( window_name_, frame );
-        cv::imshow( histogram_name_, histimg );
-
-        char c = (char)cv::waitKey(1);
-        //if( c == 27 )
-        //  break;
-        switch(c)
-        {
-          case 'b':
-            backprojMode = !backprojMode;
-            break;
-          case 'c':
-            trackObject = 0;
-            histimg = cv::Scalar::all(0);
-            break;
-          case 'h':
-            showHist = !showHist;
-            if( !showHist )
-              cv::destroyWindow( histogram_name_ );
-            else
-              cv::namedWindow( histogram_name_, 1 );
-            break;
-          case 'p':
-            paused = !paused;
-            break;
-          default:
-            ;
-        }
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img1 = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg();
-      sensor_msgs::Image::Ptr out_img2 = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, backproj).toImageMsg();
-      img_pub_.publish(out_img1);
-      bproj_pub_.publish(out_img2);
-      if( trackObject )
-        msg_pub_.publish(rect_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &CamShiftNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &CamShiftNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "CamShift Demo";
-    histogram_name_ = "Histogram";
-
-    vmin_ = 10; vmax_ = 256; smin_ = 30;
-    backprojMode = false;
-    selectObject = false;
-    trackObject = 0;
-    showHist = true;
-    paused = false;
-
-    hsize = 16;
-    hranges[0] = 0;
-    hranges[1] = 180;
-    phranges = hranges;
-    histimg = cv::Mat::zeros(200, 320, CV_8UC3);
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&CamShiftNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&CamShiftNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&CamShiftNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&CamShiftNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    bproj_pub_ = image_transport::ImageTransport(local_nh_).advertise("back_project", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::RotatedRectStamped>("track_box", 1, msg_connect_cb, msg_disconnect_cb);
-        
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<camshift::CamShiftConfig>::CallbackType f =
-      boost::bind(&CamShiftNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-
-    NODELET_INFO("Hot keys: ");
-    NODELET_INFO("\tESC - quit the program");
-    NODELET_INFO("\tc - stop the tracking");
-    NODELET_INFO("\tb - switch to/from backprojection view");
-    NODELET_INFO("\th - show/hide object histogram");
-    NODELET_INFO("\tp - pause video");
-    NODELET_INFO("To initialize tracking, select the object with mouse");
-
-    std::vector<float> hist_value;
-    local_nh_.getParam("histogram", hist_value);
-    if ( hist_value.size() == hsize ) {
-      hist.create(hsize, 1, CV_32F);
-      for(int i = 0; i < hsize; i ++) { hist.at<float>(i) = hist_value[i];}
-      trackObject = 1;
-      trackWindow = cv::Rect(0, 0, 640, 480); //
-      
-
-      histimg = cv::Scalar::all(0);
-      int binW = histimg.cols / hsize;
-      cv::Mat buf(1, hsize, CV_8UC3);
-      for( int i = 0; i < hsize; i++ )
-        buf.at<cv::Vec3b>(i) = cv::Vec3b(cv::saturate_cast<uchar>(i*180./hsize), 255, 255);
-      cv::cvtColor(buf, buf, cv::COLOR_HSV2BGR);
-      
-      for( int i = 0; i < hsize; i++ )
-      {
-        int val = cv::saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
-        cv::rectangle( histimg, cv::Point(i*binW,histimg.rows),
-                       cv::Point((i+1)*binW,histimg.rows - val),
-                       cv::Scalar(buf.at<cv::Vec3b>(i)), -1, 8 );
-      }
-    }
-
-  }
-};
-bool CamShiftNodelet::need_config_update_ = false;
-bool CamShiftNodelet::on_mouse_update_ = false;
-int CamShiftNodelet::on_mouse_event_ = 0;
-int CamShiftNodelet::on_mouse_x_ = 0;
-int CamShiftNodelet::on_mouse_y_ = 0;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(camshift::CamShiftNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/contour_moments_nodelet.cpp b/opencv_apps/src/nodelet/contour_moments_nodelet.cpp
deleted file mode 100644
index 3a981d8..0000000
--- a/opencv_apps/src/nodelet/contour_moments_nodelet.cpp
+++ /dev/null
@@ -1,310 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/moments_demo.cpp
-/**
- * @function moments_demo.cpp
- * @brief Demo code to calculate moments
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/ContourMomentsConfig.h"
-#include "opencv_apps/Moment.h"
-#include "opencv_apps/MomentArray.h"
-#include "opencv_apps/MomentArrayStamped.h"
-
-namespace contour_moments {
-class ContourMomentsNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  contour_moments::ContourMomentsConfig config_;
-  dynamic_reconfigure::Server<contour_moments::ContourMomentsConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  int low_threshold_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(contour_moments::ContourMomentsConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    low_threshold_ = config_.canny_low_threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::MomentArrayStamped moments_msg;
-      moments_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat src_gray;
-      /// Convert image to gray and blur it
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
-      } else {
-        src_gray = frame;
-      }
-      cv::blur( src_gray, src_gray, cv::Size(3,3) );
-
-      /// Create window
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-
-      cv::Mat canny_output;
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-      cv::RNG rng(12345);
-
-      /// Detect edges using canny
-      cv::Canny( src_gray, canny_output, low_threshold_ , low_threshold_ *2, 3 );
-      /// Find contours
-      cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
-
-      /// Get the moments
-      std::vector<cv::Moments> mu(contours.size() );
-      for( size_t i = 0; i < contours.size(); i++ )
-      { mu[i] = moments( contours[i], false ); }
-
-      ///  Get the mass centers:
-      std::vector<cv::Point2f> mc( contours.size() );
-      for( size_t i = 0; i < contours.size(); i++ )
-      { mc[i] = cv::Point2f( static_cast<float>(mu[i].m10/mu[i].m00) , static_cast<float>(mu[i].m01/mu[i].m00) ); }
-
-      /// Draw contours
-      cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
-      for( size_t i = 0; i< contours.size(); i++ )
-      {
-        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
-        cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
-        cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
-      }
-
-      /// Calculate the area with the moments 00 and compare with the result of the OpenCV function
-      printf("\t Info: Area and Contour Length \n");
-      for( size_t i = 0; i< contours.size(); i++ )
-      {
-        NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ) );
-        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
-        cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
-        cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
-
-        opencv_apps::Moment moment_msg;
-        moment_msg.m00 = mu[i].m00;
-        moment_msg.m10 = mu[i].m10;
-        moment_msg.m01 = mu[i].m01;
-        moment_msg.m20 = mu[i].m20;
-        moment_msg.m11 = mu[i].m11;
-        moment_msg.m02 = mu[i].m02;
-        moment_msg.m30 = mu[i].m30;
-        moment_msg.m21 = mu[i].m21;
-        moment_msg.m12 = mu[i].m12;
-        moment_msg.m03 = mu[i].m03;
-        moment_msg.mu20 = mu[i].mu20;
-        moment_msg.mu11 = mu[i].mu11;
-        moment_msg.mu02 = mu[i].mu02;
-        moment_msg.mu30 = mu[i].mu30;
-        moment_msg.mu21 = mu[i].mu21;
-        moment_msg.mu12 = mu[i].mu12;
-        moment_msg.mu03 = mu[i].mu03;
-        moment_msg.nu20 = mu[i].nu20;
-        moment_msg.nu11 = mu[i].nu11;
-        moment_msg.nu02 = mu[i].nu02;
-        moment_msg.nu30 = mu[i].nu30;
-        moment_msg.nu21 = mu[i].nu21;
-        moment_msg.nu12 = mu[i].nu12;
-        moment_msg.nu03 = mu[i].nu03;
-        opencv_apps::Point2D center_msg;
-        center_msg.x = mc[i].x;
-        center_msg.y = mc[i].y;
-        moment_msg.center = center_msg;
-        moment_msg.area = cv::contourArea(contours[i]);
-        moment_msg.length = cv::arcLength(contours[i], true);
-        moments_msg.moments.push_back(moment_msg);
-      }
-
-      if( debug_view_) {
-        cv::imshow( window_name_, drawing );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", drawing).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(moments_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &ContourMomentsNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &ContourMomentsNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Contours";
-    low_threshold_ = 100; // only for canny
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&ContourMomentsNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&ContourMomentsNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&ContourMomentsNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&ContourMomentsNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::MomentArrayStamped>("moments", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<contour_moments::ContourMomentsConfig>::CallbackType f =
-      boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool ContourMomentsNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(contour_moments::ContourMomentsNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/convex_hull_nodelet.cpp b/opencv_apps/src/nodelet/convex_hull_nodelet.cpp
deleted file mode 100644
index 9b5fc4e..0000000
--- a/opencv_apps/src/nodelet/convex_hull_nodelet.cpp
+++ /dev/null
@@ -1,282 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/hull_demo.cpp
-/**
- * @function hull_demo.cpp
- * @brief Demo code to find contours in an image
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/ConvexHullConfig.h"
-#include "opencv_apps/Contour.h"
-#include "opencv_apps/ContourArray.h"
-#include "opencv_apps/ContourArrayStamped.h"
-
-namespace convex_hull {
-class ConvexHullNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  convex_hull::ConvexHullConfig config_;
-  dynamic_reconfigure::Server<convex_hull::ConvexHullConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  int threshold_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(convex_hull::ConvexHullConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    threshold_ = config_.threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::ContourArrayStamped contours_msg;
-      contours_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat src_gray;
-
-      /// Convert image to gray and blur it
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
-      } else {
-        src_gray = frame;
-      }
-      cv::blur( src_gray, src_gray, cv::Size(3,3) );
-
-      /// Create window
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-
-      cv::Mat threshold_output;
-      int max_thresh = 255;
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-      cv::RNG rng(12345);
-
-      /// Detect edges using Threshold
-      cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
-
-      /// Find contours
-      cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
-
-      /// Find the convex hull object for each contour
-      std::vector<std::vector<cv::Point> >hull( contours.size() );
-      for( size_t i = 0; i < contours.size(); i++ )
-      {   cv::convexHull( cv::Mat(contours[i]), hull[i], false ); }
-
-      /// Draw contours + hull results
-      cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
-      for( size_t i = 0; i< contours.size(); i++ )
-      {
-        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
-        cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
-        cv::drawContours( drawing, hull, (int)i, color, 4, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
-
-        opencv_apps::Contour contour_msg;
-        for ( size_t j = 0; j < hull[i].size(); j++ ) {
-          opencv_apps::Point2D point_msg;
-          point_msg.x = hull[i][j].x;
-          point_msg.y = hull[i][j].y;
-          contour_msg.points.push_back(point_msg);
-        }
-        contours_msg.contours.push_back(contour_msg);
-      }
-
-      /// Create a Trackbar for user to enter threshold
-      if( debug_view_) {
-        if (need_config_update_) {
-          config_.threshold = threshold_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-        cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
-
-        cv::imshow( window_name_, drawing );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(contours_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &ConvexHullNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &ConvexHullNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Hull Demo";
-    threshold_ = 100;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&ConvexHullNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&ConvexHullNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&ConvexHullNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&ConvexHullNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("hulls", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<convex_hull::ConvexHullConfig>::CallbackType f =
-      boost::bind(&ConvexHullNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool ConvexHullNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(convex_hull::ConvexHullNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/edge_detection_nodelet.cpp b/opencv_apps/src/nodelet/edge_detection_nodelet.cpp
deleted file mode 100644
index 0179183..0000000
--- a/opencv_apps/src/nodelet/edge_detection_nodelet.cpp
+++ /dev/null
@@ -1,321 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/
-/**
- * @file Sobel_Demo.cpp
- * @brief Sample code using Sobel and/orScharr OpenCV functions to make a simple Edge Detector
- * @author OpenCV team
- */
-/**
- * @file Laplace_Demo.cpp
- * @brief Sample code showing how to detect edges using the Laplace operator
- * @author OpenCV team
- */
-/**
- * @file CannyDetector_Demo.cpp
- * @brief Sample code showing how to detect edges using the Canny Detector
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/EdgeDetectionConfig.h"
-
-namespace edge_detection {
-class EdgeDetectionNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  edge_detection::EdgeDetectionConfig config_;
-  dynamic_reconfigure::Server<edge_detection::EdgeDetectionConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  int low_threshold_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(edge_detection::EdgeDetectionConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    low_threshold_ = config_.canny_low_threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Do the work
-      cv::Mat src_gray;
-      cv::GaussianBlur( frame, frame, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT );
-
-      /// Convert it to gray
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
-      } else {
-        src_gray = frame;
-      }
-
-      /// Create window
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-
-      std::string new_window_name;
-      cv::Mat grad;
-      switch (config_.edge_type) {
-        case edge_detection::EdgeDetection_Sobel:
-          {
-            /// Generate grad_x and grad_y
-            cv::Mat grad_x, grad_y;
-            cv::Mat abs_grad_x, abs_grad_y;
-
-            int scale = 1;
-            int delta = 0;
-            int ddepth = CV_16S;
-
-            /// Gradient X
-            //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
-            cv::Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT );
-            cv::convertScaleAbs( grad_x, abs_grad_x );
-
-            /// Gradient Y
-            //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
-            cv::Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT );
-            cv::convertScaleAbs( grad_y, abs_grad_y );
-
-            /// Total Gradient (approximate)
-            cv::addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad );
-
-            new_window_name = "Sobel Edge Detection Demo";
-            break;
-          }
-        case edge_detection::EdgeDetection_Laplace:
-          {
-            cv::Mat dst;
-            int kernel_size = 3;
-            int scale = 1;
-            int delta = 0;
-            int ddepth = CV_16S;
-            /// Apply Laplace function
-
-            cv::Laplacian( src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT );
-            convertScaleAbs( dst, grad );
-
-            new_window_name = "Laplace Edge Detection Demo";
-            break;
-          }
-        case edge_detection::EdgeDetection_Canny:
-          {
-            int edgeThresh = 1;
-            int const max_lowThreshold = 100;
-            int ratio = 3;
-            int kernel_size = 3;
-            cv::Mat detected_edges;
-
-            /// Reduce noise with a kernel 3x3
-            cv::blur( src_gray, grad, cv::Size(3,3) );
-
-            /// Canny detector
-            cv::Canny( grad, grad, low_threshold_, low_threshold_*ratio, kernel_size );
-
-            new_window_name = "Canny Edge Detection Demo";
-
-            /// Create a Trackbar for user to enter threshold
-            if( debug_view_) {
-              if (need_config_update_) {
-                config_.canny_low_threshold = low_threshold_;
-                srv.updateConfig(config_);
-                need_config_update_ = false;
-              }
-              if( window_name_ == new_window_name) {
-                cv::createTrackbar( "Min Threshold:", window_name_, &low_threshold_, max_lowThreshold, trackbarCallback);
-              }
-            }
-            break;
-          }
-      }
-
-
-      if( debug_view_) {
-        if (window_name_ != new_window_name) {
-          cv::destroyWindow(window_name_);
-          window_name_ = new_window_name;
-        }
-        cv::imshow( window_name_, grad );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::MONO8, grad).toImageMsg();
-      img_pub_.publish(out_img);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &EdgeDetectionNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &EdgeDetectionNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Edge Detection Demo";
-    low_threshold_ = 10; // only for canny
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&EdgeDetectionNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&EdgeDetectionNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&EdgeDetectionNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&EdgeDetectionNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    //msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<edge_detection::EdgeDetectionConfig>::CallbackType f =
-      boost::bind(&EdgeDetectionNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool EdgeDetectionNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(edge_detection::EdgeDetectionNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/face_detection_nodelet.cpp b/opencv_apps/src/nodelet/face_detection_nodelet.cpp
deleted file mode 100644
index a81197c..0000000
--- a/opencv_apps/src/nodelet/face_detection_nodelet.cpp
+++ /dev/null
@@ -1,271 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/objectDetection/objectDetection.cpp
-/**
- * @file objectDetection.cpp
- * @author A. Huaman ( based in the classic facedetect.cpp in samples/c )
- * @brief A simplified version of facedetect.cpp, show how to load a cascade classifier and how to find objects (Face + eyes) in a video stream
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/objdetect/objdetect.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/FaceDetectionConfig.h"
-#include "opencv_apps/Face.h"
-#include "opencv_apps/FaceArray.h"
-#include "opencv_apps/FaceArrayStamped.h"
-
-namespace face_detection {
-class FaceDetectionNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  face_detection::FaceDetectionConfig config_;
-  dynamic_reconfigure::Server<face_detection::FaceDetectionConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  cv::CascadeClassifier face_cascade_;
-  cv::CascadeClassifier eyes_cascade_;
-
-  void reconfigureCallback(face_detection::FaceDetectionConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::FaceArrayStamped faces_msg;
-      faces_msg.header = msg->header;
-
-      // Do the work
-      std::vector<cv::Rect> faces;
-      cv::Mat frame_gray;
-
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, frame_gray, cv::COLOR_BGR2GRAY );
-      } else {
-        frame_gray = frame;
-      }
-      cv::equalizeHist( frame_gray, frame_gray );
-      //-- Detect faces
-#ifndef CV_VERSION_EPOCH
-      face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0, cv::Size(30, 30) );
-#else
-      face_cascade_.detectMultiScale( frame_gray, faces, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
-#endif
-
-      for( size_t i = 0; i < faces.size(); i++ )
-      {
-        cv::Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 );
-        cv::ellipse( frame,  center, cv::Size( faces[i].width/2, faces[i].height/2), 0, 0, 360, cv::Scalar( 255, 0, 255 ), 2, 8, 0 );
-        opencv_apps::Face face_msg;
-        face_msg.face.x = center.x;
-        face_msg.face.y = center.y;
-        face_msg.face.width = faces[i].width;
-        face_msg.face.height = faces[i].height;
-
-        cv::Mat faceROI = frame_gray( faces[i] );
-        std::vector<cv::Rect> eyes;
-
-        //-- In each face, detect eyes
-#ifndef CV_VERSION_EPOCH
-        eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0, cv::Size(30, 30) );
-#else
-        eyes_cascade_.detectMultiScale( faceROI, eyes, 1.1, 2, 0 | CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
-#endif
-
-        for( size_t j = 0; j < eyes.size(); j++ )
-        {
-          cv::Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 );
-          int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
-          cv::circle( frame, eye_center, radius, cv::Scalar( 255, 0, 0 ), 3, 8, 0 );
-
-          opencv_apps::Rect eye_msg;
-          eye_msg.x = eye_center.x;
-          eye_msg.y = eye_center.y;
-          eye_msg.width = eyes[j].width;
-          eye_msg.height = eyes[j].height;
-          face_msg.eyes.push_back(eye_msg);
-        }
-
-        faces_msg.faces.push_back(face_msg);
-      }
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( "Face detection", frame );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(faces_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &FaceDetectionNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &FaceDetectionNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FaceDetectionNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FaceDetectionNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FaceDetectionNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FaceDetectionNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::FaceArrayStamped>("faces", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-    std::string face_cascade_name, eyes_cascade_name;
-    local_nh_.param("face_cascade_name", face_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml"));
-    local_nh_.param("eyes_cascade_name", eyes_cascade_name, std::string("/usr/share/opencv/haarcascades/haarcascade_eye_tree_eyeglasses.xml"));
-
-    if( !face_cascade_.load( face_cascade_name ) ){ NODELET_ERROR("--Error loading %s", face_cascade_name.c_str()); };
-    if( !eyes_cascade_.load( eyes_cascade_name ) ){ NODELET_ERROR("--Error loading %s", eyes_cascade_name.c_str()); };
-
-    dynamic_reconfigure::Server<face_detection::FaceDetectionConfig>::CallbackType f =
-      boost::bind(&FaceDetectionNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(face_detection::FaceDetectionNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/fback_flow_nodelet.cpp b/opencv_apps/src/nodelet/fback_flow_nodelet.cpp
deleted file mode 100644
index 24dbc6e..0000000
--- a/opencv_apps/src/nodelet/fback_flow_nodelet.cpp
+++ /dev/null
@@ -1,264 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/fback.cpp
-/*
- * This program demonstrates dense optical flow algorithm by Gunnar Farneback
- * Mainly the function: calcOpticalFlowFarneback()
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/video/tracking.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/FBackFlowConfig.h"
-#include "opencv_apps/FlowArrayStamped.h"
-
-namespace fback_flow {
-class FBackFlowNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  fback_flow::FBackFlowConfig config_;
-  dynamic_reconfigure::Server<fback_flow::FBackFlowConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  cv::Mat prevgray, gray, flow, cflow;
-
-  void reconfigureCallback(fback_flow::FBackFlowConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::FlowArrayStamped flows_msg;
-      flows_msg.header = msg->header;
-
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        if (need_config_update_) {
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      // Do the work
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, gray, cv::COLOR_BGR2GRAY );
-      } else {
-        frame.copyTo(gray);
-      }
-      if( prevgray.data )
-      {
-        cv::calcOpticalFlowFarneback(prevgray, gray, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
-        cv::cvtColor(prevgray, cflow, cv::COLOR_GRAY2BGR);
-        //drawOptFlowMap(flow, cflow, 16, 1.5, Scalar(0, 255, 0));
-        int step = 16;
-        cv::Scalar color = cv::Scalar(0, 255, 0);
-        for(int y = 0; y < cflow.rows; y += step)
-          for(int x = 0; x < cflow.cols; x += step)
-        {
-          const cv::Point2f& fxy = flow.at<cv::Point2f>(y, x);
-          cv::line(cflow, cv::Point(x,y), cv::Point(cvRound(x+fxy.x), cvRound(y+fxy.y)),
-                   color);
-          cv::circle(cflow, cv::Point(x,y), 2, color, -1);
-
-          opencv_apps::Flow flow_msg;
-          opencv_apps::Point2D point_msg;
-          opencv_apps::Point2D velocity_msg;
-          point_msg.x = x;
-          point_msg.y = y;
-          velocity_msg.x = fxy.x;
-          velocity_msg.y = fxy.y;
-          flow_msg.point = point_msg;
-          flow_msg.velocity = velocity_msg;
-          flows_msg.flow.push_back(flow_msg);
-        }
-      }
-
-      std::swap(prevgray, gray);
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, cflow );
-        int c = cv::waitKey(1);
-      }
-
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(flows_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &FBackFlowNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &FBackFlowNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "flow";
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FBackFlowNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FBackFlowNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FBackFlowNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FBackFlowNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::FlowArrayStamped>("flows", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<fback_flow::FBackFlowConfig>::CallbackType f =
-      boost::bind(&FBackFlowNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool FBackFlowNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(fback_flow::FBackFlowNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/find_contours_nodelet.cpp b/opencv_apps/src/nodelet/find_contours_nodelet.cpp
deleted file mode 100644
index dd1398a..0000000
--- a/opencv_apps/src/nodelet/find_contours_nodelet.cpp
+++ /dev/null
@@ -1,279 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/findContours_Demo.cpp
-/**
- * @function findContours_Demo.cpp
- * @brief Demo code to find contours in an image
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/FindContoursConfig.h"
-#include "opencv_apps/Contour.h"
-#include "opencv_apps/ContourArray.h"
-#include "opencv_apps/ContourArrayStamped.h"
-
-namespace find_contours {
-class FindContoursNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  find_contours::FindContoursConfig config_;
-  dynamic_reconfigure::Server<find_contours::FindContoursConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  int low_threshold_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(find_contours::FindContoursConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    low_threshold_ = config_.canny_low_threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::ContourArrayStamped contours_msg;
-      contours_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat src_gray;
-
-      /// Convert it to gray
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
-      } else {
-        src_gray = frame;
-      }
-      cv::GaussianBlur( src_gray, src_gray, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT );
-
-      /// Create window
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-      
-      cv::Mat canny_output;
-      int max_thresh = 255;
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-      cv::RNG rng(12345);
-
-      /// Reduce noise with a kernel 3x3
-      cv::blur( src_gray, canny_output, cv::Size(3,3) );
-
-      /// Canny detector
-      cv::Canny( canny_output, canny_output, low_threshold_, low_threshold_*2, 3 );
-
-      /// Find contours
-      cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
-
-      /// Draw contours
-      cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
-      for( size_t i = 0; i< contours.size(); i++ )
-      {
-        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
-        cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
-        
-        opencv_apps::Contour contour_msg;
-        for ( size_t j = 0; j < contours[i].size(); j++ ) {
-          opencv_apps::Point2D point_msg;
-          point_msg.x = contours[i][j].x;
-          point_msg.y = contours[i][j].y;
-          contour_msg.points.push_back(point_msg);
-        }
-        contours_msg.contours.push_back(contour_msg);
-      }
-
-      /// Create a Trackbar for user to enter threshold
-      if( debug_view_) {
-        if (need_config_update_) {
-          config_.canny_low_threshold = low_threshold_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-        cv::createTrackbar( "Canny thresh:", window_name_, &low_threshold_, max_thresh, trackbarCallback);
-
-        cv::imshow( window_name_, drawing );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(contours_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &FindContoursNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &FindContoursNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Demo code to find contours in an image";
-    low_threshold_ = 100; // only for canny
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&FindContoursNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&FindContoursNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&FindContoursNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&FindContoursNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
-        
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<find_contours::FindContoursConfig>::CallbackType f =
-      boost::bind(&FindContoursNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool FindContoursNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(find_contours::FindContoursNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/general_contours_nodelet.cpp b/opencv_apps/src/nodelet/general_contours_nodelet.cpp
deleted file mode 100644
index 46e8a86..0000000
--- a/opencv_apps/src/nodelet/general_contours_nodelet.cpp
+++ /dev/null
@@ -1,311 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ShapeDescriptors/generalContours_demo2.cpp
-/**
- * @function generalContours_demo2.cpp
- * @brief Demo code to obtain ellipses and rotated rectangles that contain detected contours
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <sensor_msgs/image_encodings.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/GeneralContoursConfig.h"
-#include "opencv_apps/RotatedRect.h"
-#include "opencv_apps/RotatedRectArray.h"
-#include "opencv_apps/RotatedRectArrayStamped.h"
-
-namespace general_contours {
-class GeneralContoursNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher rects_pub_, ellipses_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  general_contours::GeneralContoursConfig config_;
-  dynamic_reconfigure::Server<general_contours::GeneralContoursConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  int threshold_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(general_contours::GeneralContoursConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    threshold_ = config_.threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::RotatedRectArrayStamped rects_msg, ellipses_msg;
-      rects_msg.header = msg->header;
-      ellipses_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat src_gray;
-
-      /// Convert image to gray and blur it
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_RGB2GRAY );
-      } else {
-        src_gray = frame;
-      }
-      cv::blur( src_gray, src_gray, cv::Size(3,3) );
-
-      /// Create window
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-
-      cv::Mat threshold_output;
-      int max_thresh = 255;
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-      cv::RNG rng(12345);
-
-      /// Detect edges using Threshold
-      cv::threshold( src_gray, threshold_output, threshold_, 255, cv::THRESH_BINARY );
-
-      /// Find contours
-      cv::findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
-
-      /// Find the rotated rectangles and ellipses for each contour
-      std::vector<cv::RotatedRect> minRect( contours.size() );
-      std::vector<cv::RotatedRect> minEllipse( contours.size() );
-
-      for( size_t i = 0; i < contours.size(); i++ )
-      { minRect[i] = cv::minAreaRect( cv::Mat(contours[i]) );
-        if( contours[i].size() > 5 )
-        { minEllipse[i] = cv::fitEllipse( cv::Mat(contours[i]) ); }
-      }
-
-      /// Draw contours + rotated rects + ellipses
-      cv::Mat drawing = cv::Mat::zeros( threshold_output.size(), CV_8UC3 );
-      for( size_t i = 0; i< contours.size(); i++ )
-      {
-        cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
-        // contour
-        cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
-        // ellipse
-        cv::ellipse( drawing, minEllipse[i], color, 2, 8 );
-        // rotated rectangle
-        cv::Point2f rect_points[4]; minRect[i].points( rect_points );
-        for( int j = 0; j < 4; j++ )
-          cv::line( drawing, rect_points[j], rect_points[(j+1)%4], color, 1, 8 );
-
-        opencv_apps::RotatedRect rect_msg;
-        opencv_apps::Point2D rect_center;
-        opencv_apps::Size rect_size;
-        rect_center.x = minRect[i].center.x;
-        rect_center.y = minRect[i].center.y;
-        rect_size.width = minRect[i].size.width;
-        rect_size.height = minRect[i].size.height;
-        rect_msg.center = rect_center;
-        rect_msg.size = rect_size;
-        rect_msg.angle = minRect[i].angle;
-        opencv_apps::RotatedRect ellipse_msg;
-        opencv_apps::Point2D ellipse_center;
-        opencv_apps::Size ellipse_size;
-        ellipse_center.x = minEllipse[i].center.x;
-        ellipse_center.y = minEllipse[i].center.y;
-        ellipse_size.width = minEllipse[i].size.width;
-        ellipse_size.height = minEllipse[i].size.height;
-        ellipse_msg.center = ellipse_center;
-        ellipse_msg.size = ellipse_size;
-        ellipse_msg.angle = minEllipse[i].angle;
-
-        rects_msg.rects.push_back(rect_msg);
-        ellipses_msg.rects.push_back(ellipse_msg);
-      }
-
-      /// Create a Trackbar for user to enter threshold
-      if( debug_view_) {
-        if (need_config_update_) {
-          config_.threshold = threshold_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-        cv::createTrackbar( "Threshold:", window_name_, &threshold_, max_thresh, trackbarCallback);
-
-        cv::imshow( window_name_, drawing );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, drawing).toImageMsg();
-      img_pub_.publish(out_img);
-      rects_pub_.publish(rects_msg);
-      ellipses_pub_.publish(ellipses_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &GeneralContoursNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &GeneralContoursNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Contours";
-    threshold_ = 100;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&GeneralContoursNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&GeneralContoursNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&GeneralContoursNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&GeneralContoursNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    rects_pub_ = local_nh_.advertise<opencv_apps::RotatedRectArrayStamped>("rectangles", 1, msg_connect_cb, msg_disconnect_cb);
-    ellipses_pub_ = local_nh_.advertise<opencv_apps::RotatedRectArrayStamped>("ellipses", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<general_contours::GeneralContoursConfig>::CallbackType f =
-      boost::bind(&GeneralContoursNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool GeneralContoursNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(general_contours::GeneralContoursNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp b/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp
deleted file mode 100644
index e37704a..0000000
--- a/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp
+++ /dev/null
@@ -1,282 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// http://github.com/Itseez/opencv/blob/master/samples/cpp/tutorial_code/TrackingMotion/goodFeaturesToTrack_Demo.cpp
-/**
- * @function goodFeaturesToTrack_Demo.cpp
- * @brief Demo code for detecting corners using Shi-Tomasi method
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/GoodfeatureTrackConfig.h"
-#include "opencv_apps/Point2D.h"
-#include "opencv_apps/Point2DArrayStamped.h"
-
-namespace goodfeature_track {
-class GoodfeatureTrackNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  goodfeature_track::GoodfeatureTrackConfig config_;
-  dynamic_reconfigure::Server<goodfeature_track::GoodfeatureTrackConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  int max_corners_;
-
-  void reconfigureCallback(goodfeature_track::GoodfeatureTrackConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    max_corners_ = config_.max_corners;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::Point2DArrayStamped corners_msg;
-      corners_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat src_gray;
-      int maxTrackbar = 100;
-
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
-      } else {
-        src_gray = frame;
-        cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR );
-      }
-
-      if( debug_view_) {
-        /// Create Trackbars for Thresholds
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        cv::createTrackbar( "Max corners", window_name_, &max_corners_, maxTrackbar, trackbarCallback);
-        if (need_config_update_) {
-          config_.max_corners = max_corners_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      /// void goodFeaturesToTrack_Demo( int, void* )
-      if( max_corners_ < 1 ) { max_corners_ = 1; }
-
-      /// Parameters for Shi-Tomasi algorithm
-      std::vector<cv::Point2f> corners;
-      double qualityLevel = 0.01;
-      double minDistance = 10;
-      int blockSize = 3;
-      bool useHarrisDetector = false;
-      double k = 0.04;
-
-      cv::RNG rng(12345);
-
-      /// Apply corner detection
-      cv::goodFeaturesToTrack( src_gray,
-                               corners,
-                               max_corners_,
-                               qualityLevel,
-                               minDistance,
-                               cv::Mat(),
-                               blockSize,
-                               useHarrisDetector,
-                               k );
-
-
-      /// Draw corners detected
-      NODELET_INFO_STREAM("** Number of corners detected: "<<corners.size());
-      int r = 4;
-      for( size_t i = 0; i < corners.size(); i++ )
-      { cv::circle( frame, corners[i], r, cv::Scalar(rng.uniform(0,255), rng.uniform(0,255), rng.uniform(0,255)), -1, 8, 0 ); }
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      // Create msgs
-      for( size_t i = 0; i< corners.size(); i++ ) {
-        opencv_apps::Point2D corner;
-        corner.x = corners[i].x;
-        corner.y = corners[i].y;
-        corners_msg.points.push_back(corner);
-      }
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(corners_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &GoodfeatureTrackNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &GoodfeatureTrackNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Image";
-    max_corners_ = 23;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&GoodfeatureTrackNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&GoodfeatureTrackNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&GoodfeatureTrackNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&GoodfeatureTrackNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::Point2DArrayStamped>("corners", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<goodfeature_track::GoodfeatureTrackConfig>::CallbackType f =
-      boost::bind(&GoodfeatureTrackNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool GoodfeatureTrackNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(goodfeature_track::GoodfeatureTrackNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/hough_circles_nodelet.cpp b/opencv_apps/src/nodelet/hough_circles_nodelet.cpp
deleted file mode 100644
index 99becfa..0000000
--- a/opencv_apps/src/nodelet/hough_circles_nodelet.cpp
+++ /dev/null
@@ -1,295 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughCircle_Demo.cpp
-/**
- * @file HoughCircle_Demo.cpp
- * @brief Demo code for Hough Transform
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/HoughCirclesConfig.h"
-#include "opencv_apps/Circle.h"
-#include "opencv_apps/CircleArray.h"
-#include "opencv_apps/CircleArrayStamped.h"
-
-namespace hough_circles {
-class HoughCirclesNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  hough_circles::HoughCirclesConfig config_;
-  dynamic_reconfigure::Server<hough_circles::HoughCirclesConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  // initial and max values of the parameters of interests.
-  int canny_threshold_initial_value_;
-  int accumulator_threshold_initial_value_;
-  int max_accumulator_threshold_;
-  int max_canny_threshold_;
-  int canny_threshold_;
-  int accumulator_threshold_;
-
-
-  void reconfigureCallback(hough_circles::HoughCirclesConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    canny_threshold_ = config_.canny_threshold;
-    accumulator_threshold_ = config_.accumulator_threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::CircleArrayStamped circles_msg;
-      circles_msg.header = msg->header;
-
-      // Do the work
-      std::vector<cv::Rect> faces;
-      cv::Mat src_gray, edges;
-
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
-      } else {
-        src_gray = frame;
-      }
-
-      // Reduce the noise so we avoid false circle detection
-      cv::GaussianBlur( src_gray, src_gray, cv::Size(9, 9), 2, 2 );
-
-      // create the main window, and attach the trackbars
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-
-        cv::createTrackbar("Canny Threshold", window_name_, &canny_threshold_, max_canny_threshold_, trackbarCallback);
-        cv::createTrackbar("Accumulator Threshold", window_name_, &accumulator_threshold_, max_accumulator_threshold_, trackbarCallback);
-
-        if (need_config_update_) {
-          config_.canny_threshold = canny_threshold_;
-          config_.accumulator_threshold = accumulator_threshold_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      // those paramaters cannot be =0
-      // so we must check here
-      canny_threshold_ = std::max(canny_threshold_, 1);
-      accumulator_threshold_ = std::max(accumulator_threshold_, 1);
-
-      //runs the detection, and update the display
-      // will hold the results of the detection
-      std::vector<cv::Vec3f> circles;
-      // runs the actual detection
-      cv::HoughCircles( src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows/8, canny_threshold_, accumulator_threshold_, 0, 0 );
-
-
-      // clone the colour, input image for displaying purposes
-      for( size_t i = 0; i < circles.size(); i++ )
-      {
-        cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
-        int radius = cvRound(circles[i][2]);
-        // circle center
-        circle( frame, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
-        // circle outline
-        circle( frame, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
-
-        opencv_apps::Circle circle_msg;
-        circle_msg.center.x = center.x;
-        circle_msg.center.y = center.y;
-        circle_msg.radius = radius;
-        circles_msg.circles.push_back(circle_msg);
-      }
-
-      // shows the results
-      if( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(circles_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &HoughCirclesNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &HoughCirclesNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Hough Circle Detection Demo";
-    canny_threshold_initial_value_ = 200;
-    accumulator_threshold_initial_value_ = 50;
-    max_accumulator_threshold_ = 200;
-    max_canny_threshold_ = 255;
-
-    //declare and initialize both parameters that are subjects to change
-    canny_threshold_ = canny_threshold_initial_value_;
-    accumulator_threshold_ = accumulator_threshold_initial_value_;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&HoughCirclesNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughCirclesNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&HoughCirclesNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughCirclesNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::CircleArrayStamped>("circles", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<hough_circles::HoughCirclesConfig>::CallbackType f =
-      boost::bind(&HoughCirclesNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool HoughCirclesNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(hough_circles::HoughCirclesNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/hough_lines_nodelet.cpp b/opencv_apps/src/nodelet/hough_lines_nodelet.cpp
deleted file mode 100644
index d12694f..0000000
--- a/opencv_apps/src/nodelet/hough_lines_nodelet.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughLines_Demo.cpp
-/**
- * @file HoughLines_Demo.cpp
- * @brief Demo code for Hough Transform
- * @author OpenCV team
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/HoughLinesConfig.h"
-#include "opencv_apps/Line.h"
-#include "opencv_apps/LineArray.h"
-#include "opencv_apps/LineArrayStamped.h"
-
-namespace hough_lines {
-class HoughLinesNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  hough_lines::HoughLinesConfig config_;
-  dynamic_reconfigure::Server<hough_lines::HoughLinesConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  int min_threshold_;
-  int max_threshold_;
-  int threshold_;
-
-  void reconfigureCallback(hough_lines::HoughLinesConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    threshold_ = config_.threshold;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::LineArrayStamped lines_msg;
-      lines_msg.header = msg->header;
-
-      // Do the work
-      std::vector<cv::Rect> faces;
-      cv::Mat src_gray, edges;
-
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
-      } else {
-        src_gray = frame;
-      }
-      /// Apply Canny edge detector
-      Canny( src_gray, edges, 50, 200, 3 );
-
-      if( debug_view_) {
-        /// Create Trackbars for Thresholds
-        char thresh_label[50];
-        sprintf( thresh_label, "Thres: %d + input", min_threshold_ );
-
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        cv::createTrackbar( thresh_label, window_name_, &threshold_, max_threshold_, trackbarCallback);
-        if (need_config_update_) {
-          config_.threshold = threshold_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      /// Initialize
-      cv::cvtColor( edges, frame, cv::COLOR_GRAY2BGR );
-
-      switch (config_.hough_type) {
-        case hough_lines::HoughLines_Standard_Hough_Transform:
-          {
-            std::vector<cv::Vec2f> s_lines;
-
-            /// 1. Use Standard Hough Transform
-            cv::HoughLines( edges, s_lines, 1, CV_PI/180, min_threshold_ + threshold_, 0, 0 );
-
-            /// Show the result
-            for( size_t i = 0; i < s_lines.size(); i++ )
-            {
-              float r = s_lines[i][0], t = s_lines[i][1];
-              double cos_t = cos(t), sin_t = sin(t);
-              double x0 = r*cos_t, y0 = r*sin_t;
-              double alpha = 1000;
-
-              cv::Point pt1( cvRound(x0 + alpha*(-sin_t)), cvRound(y0 + alpha*cos_t) );
-              cv::Point pt2( cvRound(x0 - alpha*(-sin_t)), cvRound(y0 - alpha*cos_t) );
-#ifndef CV_VERSION_EPOCH
-              cv::line( frame, pt1, pt2, cv::Scalar(255,0,0), 3, cv::LINE_AA);
-#else
-              cv::line( frame, pt1, pt2, cv::Scalar(255,0,0), 3, CV_AA);
-#endif
-              opencv_apps::Line line_msg;
-              line_msg.pt1.x = pt1.x;
-              line_msg.pt1.y = pt1.y;
-              line_msg.pt2.x = pt2.x;
-              line_msg.pt2.y = pt2.y;
-              lines_msg.lines.push_back(line_msg);
-            }
-
-            break;
-          }
-        case hough_lines::HoughLines_Probabilistic_Hough_Transform:
-        default:
-          {
-            std::vector<cv::Vec4i> p_lines;
-
-            /// 2. Use Probabilistic Hough Transform
-            cv::HoughLinesP( edges, p_lines, 1, CV_PI/180, min_threshold_ + threshold_, 30, 10 );
-
-            /// Show the result
-            for( size_t i = 0; i < p_lines.size(); i++ )
-            {
-              cv::Vec4i l = p_lines[i];
-#ifndef CV_VERSION_EPOCH
-              cv::line( frame, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, cv::LINE_AA);
-#else
-              cv::line( frame, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(255,0,0), 3, CV_AA);
-#endif
-              opencv_apps::Line line_msg;
-              line_msg.pt1.x = l[0];
-              line_msg.pt1.y = l[1];
-              line_msg.pt2.x = l[2];
-              line_msg.pt2.y = l[3];
-              lines_msg.lines.push_back(line_msg);
-            }
-
-            break;
-          }
-      }
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(lines_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &HoughLinesNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &HoughLinesNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "Hough Lines Demo";
-    min_threshold_ = 50;
-    max_threshold_ = 150;
-    threshold_ = max_threshold_;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&HoughLinesNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&HoughLinesNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&HoughLinesNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&HoughLinesNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<hough_lines::HoughLinesConfig>::CallbackType f =
-      boost::bind(&HoughLinesNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool HoughLinesNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(hough_lines::HoughLinesNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/lk_flow_nodelet.cpp b/opencv_apps/src/nodelet/lk_flow_nodelet.cpp
deleted file mode 100644
index c0ae5ed..0000000
--- a/opencv_apps/src/nodelet/lk_flow_nodelet.cpp
+++ /dev/null
@@ -1,370 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/lk_demo.cpp
-/**
- * This is a demo of Lukas-Kanade optical flow lkdemo(),
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/video/tracking.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "std_srvs/Empty.h"
-#include "opencv_apps/LKFlowConfig.h"
-#include "opencv_apps/FlowArrayStamped.h"
-
-namespace lk_flow {
-class LKFlowNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-  ros::ServiceServer initialize_points_service_;
-  ros::ServiceServer delete_points_service_;
-  ros::ServiceServer toggle_night_mode_service_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  lk_flow::LKFlowConfig config_;
-  dynamic_reconfigure::Server<lk_flow::LKFlowConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  int MAX_COUNT;
-  bool needToInit;
-  bool nightMode;
-  cv::Point2f point;
-  bool addRemovePt;
-  cv::Mat gray, prevGray;
-  std::vector<cv::Point2f> points[2];
-
-  void reconfigureCallback(lk_flow::LKFlowConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-#if 0
-  static void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ )
-  {
-    if( event == CV_EVENT_LBUTTONDOWN )
-    {
-      point = Point2f((float)x, (float)y);
-      addRemovePt = true;
-    }
-  }
-#endif
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat image = cv_bridge::toCvShare(msg, msg->encoding)->image;
-      if (msg->encoding == sensor_msgs::image_encodings::RGB8 ||
-          msg->encoding == sensor_msgs::image_encodings::RGB16) {
-        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
-      }
-
-      // Messages
-      opencv_apps::FlowArrayStamped flows_msg;
-      flows_msg.header = msg->header;
-
-      if( debug_view_) {
-        /// Create Trackbars for Thresholds
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        //cv::setMouseCallback( window_name_, onMouse, 0 );
-        if (need_config_update_) {
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      // Do the work
-      cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 20, 0.03);
-      cv::Size subPixWinSize(10,10), winSize(31,31);
-
-      if ( image.channels() > 1 ) {
-        cv::cvtColor( image, gray, cv::COLOR_BGR2GRAY );
-      } else {
-        image.copyTo(gray);
-      }
-
-      if( nightMode )
-        image = cv::Scalar::all(0);
-
-      if( needToInit )
-      {
-        // automatic initialization
-        cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, cv::Mat(), 3, 0, 0.04);
-        cv::cornerSubPix(gray, points[1], subPixWinSize, cv::Size(-1,-1), termcrit);
-        addRemovePt = false;
-      }
-      else if( !points[0].empty() )
-      {
-        std::vector<uchar> status;
-        std::vector<float> err;
-        if(prevGray.empty())
-          gray.copyTo(prevGray);
-        cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize,
-                                 3, termcrit, 0, 0.001);
-        size_t i, k;
-        for( i = k = 0; i < points[1].size(); i++ )
-        {
-          if( addRemovePt )
-          {
-            if( cv::norm(point - points[1][i]) <= 5 )
-            {
-              addRemovePt = false;
-              continue;
-            }
-          }
-
-          if( !status[i] )
-            continue;
-
-          points[1][k++] = points[1][i];
-          cv::circle( image, points[1][i], 3, cv::Scalar(0,255,0), -1, 8);
-          cv::line( image, points[1][i], points[0][i], cv::Scalar(0,255,0), 1, 8, 0);
-
-          opencv_apps::Flow flow_msg;
-          opencv_apps::Point2D point_msg;
-          opencv_apps::Point2D velocity_msg;
-          point_msg.x = points[1][i].x;
-          point_msg.y = points[1][i].y;
-          velocity_msg.x = points[1][i].x-points[0][i].x;
-          velocity_msg.y = points[1][i].y-points[0][i].y;
-          flow_msg.point = point_msg;
-          flow_msg.velocity = velocity_msg;
-          flows_msg.flow.push_back(flow_msg);
-        }
-        points[1].resize(k);
-      }
-
-      if( addRemovePt && points[1].size() < (size_t)MAX_COUNT )
-        {
-          std::vector<cv::Point2f> tmp;
-          tmp.push_back(point);
-          cv::cornerSubPix( gray, tmp, winSize, cv::Size(-1,-1), termcrit);
-          points[1].push_back(tmp[0]);
-          addRemovePt = false;
-        }
-
-      needToInit = false;
-      if( debug_view_) {
-
-        cv::imshow(window_name_, image);
-
-        char c = (char)cv::waitKey(1);
-        //if( c == 27 )
-        //    break;
-        switch( c )
-        {
-          case 'r':
-            needToInit = true;
-            break;
-          case 'c':
-            points[0].clear();
-            points[1].clear();
-            break;
-          case 'n':
-            nightMode = !nightMode;
-            break;
-        }
-      }
-      std::swap(points[1], points[0]);
-      cv::swap(prevGray, gray);
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, image).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(flows_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  bool initialize_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
-    needToInit = true;
-    return true;
-  }
-
-  bool delete_points_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
-    points[0].clear();
-    points[1].clear();
-    return true;
-  }
-
-  bool toggle_night_mode_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
-    nightMode = !nightMode;
-    return true;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &LKFlowNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &LKFlowNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "LK Demo";
-    MAX_COUNT = 500;
-    needToInit = true;
-    nightMode = false;
-    addRemovePt = false;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&LKFlowNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&LKFlowNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&LKFlowNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&LKFlowNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::FlowArrayStamped>("flows", 1, msg_connect_cb, msg_disconnect_cb);
-    initialize_points_service_ = local_nh_.advertiseService("initialize_points", &LKFlowNodelet::initialize_points_cb, this);
-    delete_points_service_ = local_nh_.advertiseService("delete_points", &LKFlowNodelet::delete_points_cb, this);
-    toggle_night_mode_service_ = local_nh_.advertiseService("toggle_night_mode", &LKFlowNodelet::toggle_night_mode_cb, this);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<lk_flow::LKFlowConfig>::CallbackType f =
-      boost::bind(&LKFlowNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-
-    NODELET_INFO("Hot keys: ");
-    NODELET_INFO("\tESC - quit the program");
-    NODELET_INFO("\tr - auto-initialize tracking");
-    NODELET_INFO("\tc - delete all the points");
-    NODELET_INFO("\tn - switch the \"night\" mode on/off");
-    //NODELET_INFO("To add/remove a feature point click it");
-  }
-};
-bool LKFlowNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(lk_flow::LKFlowNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/people_detect_nodelet.cpp b/opencv_apps/src/nodelet/people_detect_nodelet.cpp
deleted file mode 100644
index e6c7ae7..0000000
--- a/opencv_apps/src/nodelet/people_detect_nodelet.cpp
+++ /dev/null
@@ -1,264 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/peopledetect.cpp
-/**
- * Demonstrate the use of the HoG descriptor using
- * HOGDescriptor::hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/objdetect/objdetect.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/PeopleDetectConfig.h"
-#include "opencv_apps/Rect.h"
-#include "opencv_apps/RectArrayStamped.h"
-
-namespace people_detect {
-class PeopleDetectNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  people_detect::PeopleDetectConfig config_;
-  dynamic_reconfigure::Server<people_detect::PeopleDetectConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  cv::HOGDescriptor hog_;
-
-  void reconfigureCallback(people_detect::PeopleDetectConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::RectArrayStamped found_msg;
-      found_msg.header = msg->header;
-
-      // Do the work
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-      }
-
-      std::vector<cv::Rect> found, found_filtered;
-      double t = (double)cv::getTickCount();
-      // run the detector with default parameters. to get a higher hit-rate
-      // (and more false alarms, respectively), decrease the hitThreshold and
-      // groupThreshold (set groupThreshold to 0 to turn off the grouping completely).
-      hog_.detectMultiScale(frame, found, 0, cv::Size(8,8), cv::Size(32,32), 1.05, 2);
-      t = (double)cv::getTickCount() - t;
-      NODELET_INFO("tdetection time = %gms", t*1000./cv::getTickFrequency());
-      size_t i, j;
-      for( i = 0; i < found.size(); i++ )
-      {
-        cv::Rect r = found[i];
-        for( j = 0; j < found.size(); j++ )
-          if( j != i && (r & found[j]) == r)
-            break;
-        if( j == found.size() )
-          found_filtered.push_back(r);
-      }
-      for( i = 0; i < found_filtered.size(); i++ )
-      {
-        cv::Rect r = found_filtered[i];
-        // the HOG detector returns slightly larger rectangles than the real objects.
-        // so we slightly shrink the rectangles to get a nicer output.
-        r.x += cvRound(r.width*0.1);
-        r.width = cvRound(r.width*0.8);
-        r.y += cvRound(r.height*0.07);
-        r.height = cvRound(r.height*0.8);
-        cv::rectangle(frame, r.tl(), r.br(), cv::Scalar(0,255,0), 3);
-
-        opencv_apps::Rect rect_msg;
-        rect_msg.x = r.x;
-        rect_msg.y = r.y;
-        rect_msg.width = r.width;
-        rect_msg.height = r.height;
-        found_msg.rects.push_back(rect_msg);
-      }
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(found_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &PeopleDetectNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &PeopleDetectNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "people detector";
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&PeopleDetectNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&PeopleDetectNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&PeopleDetectNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&PeopleDetectNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::RectArrayStamped>("found", 1, msg_connect_cb, msg_disconnect_cb);
-        
-    hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<people_detect::PeopleDetectConfig>::CallbackType f =
-      boost::bind(&PeopleDetectNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool PeopleDetectNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(people_detect::PeopleDetectNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/phase_corr_nodelet.cpp b/opencv_apps/src/nodelet/phase_corr_nodelet.cpp
deleted file mode 100644
index 00ae1af..0000000
--- a/opencv_apps/src/nodelet/phase_corr_nodelet.cpp
+++ /dev/null
@@ -1,261 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/master/samples/cpp/phase_corr.cpp
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/PhaseCorrConfig.h"
-#include "opencv_apps/Point2DStamped.h"
-
-namespace phase_corr {
-class PhaseCorrNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  phase_corr::PhaseCorrConfig config_;
-  dynamic_reconfigure::Server<phase_corr::PhaseCorrConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  cv::Mat curr, prev, curr64f, prev64f, hann;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-  void reconfigureCallback(phase_corr::PhaseCorrConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::Point2DStamped shift_msg;
-      shift_msg.header = msg->header;
-
-      // Do the work
-      if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, curr, cv::COLOR_BGR2GRAY );
-      } else {
-        curr = frame;
-      }
-
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        if (need_config_update_) {
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      if(prev.empty())
-      {
-        prev = curr.clone();
-        cv::createHanningWindow(hann, curr.size(), CV_64F);
-      }
-
-      prev.convertTo(prev64f, CV_64F);
-      curr.convertTo(curr64f, CV_64F);
-
-      cv::Point2d shift = cv::phaseCorrelate(prev64f, curr64f, hann);
-      double radius = cv::sqrt(shift.x*shift.x + shift.y*shift.y);
-
-      if(radius > 0)
-      {
-        // draw a circle and line indicating the shift direction...
-        cv::Point center(curr.cols >> 1, curr.rows >> 1);
-#ifndef CV_VERSION_EPOCH
-        cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
-        cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
-#else
-        cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, CV_AA);
-        cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, CV_AA);
-#endif
-
-        //
-        shift_msg.point.x = shift.x;
-        shift_msg.point.y = shift.y;
-      }
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      prev = curr.clone();
-
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(shift_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &PhaseCorrNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &PhaseCorrNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "phase shift";
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&PhaseCorrNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&PhaseCorrNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&PhaseCorrNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&PhaseCorrNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::Point2DStamped>("shift", 1, msg_connect_cb, msg_disconnect_cb);
-
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<phase_corr::PhaseCorrConfig>::CallbackType f =
-      boost::bind(&PhaseCorrNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool PhaseCorrNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(phase_corr::PhaseCorrNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/segment_objects_nodelet.cpp b/opencv_apps/src/nodelet/segment_objects_nodelet.cpp
deleted file mode 100644
index 6c1012b..0000000
--- a/opencv_apps/src/nodelet/segment_objects_nodelet.cpp
+++ /dev/null
@@ -1,318 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/segment_objects.cpp
-/**
- * This program demonstrated a simple method of connected components clean up of background subtraction
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/video/background_segm.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/SegmentObjectsConfig.h"
-#include "std_srvs/Empty.h"
-#include "std_msgs/Float64.h"
-#include "opencv_apps/Contour.h"
-#include "opencv_apps/ContourArray.h"
-#include "opencv_apps/ContourArrayStamped.h"
-
-namespace segment_objects {
-class SegmentObjectsNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_, area_pub_;
-  ros::ServiceServer update_bg_model_service_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  segment_objects::SegmentObjectsConfig config_;
-  dynamic_reconfigure::Server<segment_objects::SegmentObjectsConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-
-#ifndef CV_VERSION_EPOCH
-  cv::Ptr<cv::BackgroundSubtractorMOG2> bgsubtractor;
-#else
-  cv::BackgroundSubtractorMOG bgsubtractor;
-#endif
-  bool update_bg_model;
-
-  void reconfigureCallback(segment_objects::SegmentObjectsConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::ContourArrayStamped contours_msg;
-      contours_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat bgmask, out_frame;
-
-      if( debug_view_) {
-        /// Create Trackbars for Thresholds
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        if (need_config_update_) {
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-#ifndef CV_VERSION_EPOCH
-      bgsubtractor->apply(frame, bgmask, update_bg_model ? -1 : 0);
-#else
-      bgsubtractor(frame, bgmask, update_bg_model ? -1 : 0);
-#endif
-      //refineSegments(tmp_frame, bgmask, out_frame);
-      int niters = 3;
-
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-
-      cv::Mat temp;
-
-      cv::dilate(bgmask, temp, cv::Mat(), cv::Point(-1,-1), niters);
-      cv::erode(temp, temp, cv::Mat(), cv::Point(-1,-1), niters*2);
-      cv::dilate(temp, temp, cv::Mat(), cv::Point(-1,-1), niters);
-
-      cv::findContours( temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
-
-      out_frame = cv::Mat::zeros(frame.size(), CV_8UC3);
-
-      if( contours.size() == 0 )
-        return;
-
-      // iterate through all the top-level contours,
-      // draw each connected component with its own random color
-      int idx = 0, largestComp = 0;
-      double maxArea = 0;
-
-      for( ; idx >= 0; idx = hierarchy[idx][0] )
-      {
-        const std::vector<cv::Point>& c = contours[idx];
-        double area = fabs(cv::contourArea(cv::Mat(c)));
-        if( area > maxArea )
-        {
-          maxArea = area;
-          largestComp = idx;
-        }
-      }
-      cv::Scalar color( 0, 0, 255 );
-      cv::drawContours( out_frame, contours, largestComp, color, CV_FILLED, 8, hierarchy );
-
-      std_msgs::Float64 area_msg;
-      area_msg.data = maxArea;
-      for( size_t i = 0; i< contours.size(); i++ ) {
-        opencv_apps::Contour contour_msg;
-        for ( size_t j = 0; j < contours[i].size(); j++ ) {
-          opencv_apps::Point2D point_msg;
-          point_msg.x = contours[i][j].x;
-          point_msg.y = contours[i][j].y;
-          contour_msg.points.push_back(point_msg);
-        }
-        contours_msg.contours.push_back(contour_msg);
-      }
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( window_name_, out_frame );
-        int keycode = cv::waitKey(1);
-        //if( keycode == 27 )
-        //    break;
-        if( keycode == ' ' )
-        {
-            update_bg_model = !update_bg_model;
-            NODELET_INFO("Learn background is in state = %d",update_bg_model);
-        }
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(contours_msg);
-      area_pub_.publish(area_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-  
-  bool update_bg_model_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
-    update_bg_model = !update_bg_model;
-    NODELET_INFO("Learn background is in state = %d",update_bg_model);
-    return true;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &SegmentObjectsNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &SegmentObjectsNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "segmented";
-    update_bg_model = true;
-
-#ifndef CV_VERSION_EPOCH
-    bgsubtractor = cv::createBackgroundSubtractorMOG2();
-#else
-    bgsubtractor.set("noiseSigma", 10);
-#endif
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&SegmentObjectsNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&SegmentObjectsNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&SegmentObjectsNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&SegmentObjectsNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
-    area_pub_ = local_nh_.advertise<std_msgs::Float64>("area", 1, msg_connect_cb, msg_disconnect_cb);
-    update_bg_model_service_ = local_nh_.advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this);
-        
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<segment_objects::SegmentObjectsConfig>::CallbackType f =
-      boost::bind(&SegmentObjectsNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool SegmentObjectsNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(segment_objects::SegmentObjectsNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/simple_compressed_example_nodelet.cpp b/opencv_apps/src/nodelet/simple_compressed_example_nodelet.cpp
deleted file mode 100644
index 85ec9fa..0000000
--- a/opencv_apps/src/nodelet/simple_compressed_example_nodelet.cpp
+++ /dev/null
@@ -1,121 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2015, Tal Regev.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-//http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
-/**
- * This is a demo of Simple Example from wiki tutorial
- */
-
-#include <ros/ros.h>
-#include <sensor_msgs/image_encodings.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-
-#include <dynamic_reconfigure/server.h>
-
-
-namespace simple_compressed_example {
-
-static const std::string OPENCV_WINDOW = "Image window";
-
-class ImageConverter
-{
-  ros::NodeHandle nh_;
-  ros::Subscriber image_sub_;
-  ros::Publisher image_pub_;
-
-public:
-  ImageConverter()
-  {
-      // Subscrive to input video feed and publish output video feed
-    image_sub_ = nh_.subscribe("image/compressed", 1,
-            &ImageConverter::imageCb,this);
-    image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>("/image_converter/output_video/compressed", 1);
-
-    cv::namedWindow(OPENCV_WINDOW);
-  }
-
-  ~ImageConverter()
-  {
-    cv::destroyWindow(OPENCV_WINDOW);
-  }
-
-  void imageCb(const sensor_msgs::CompressedImageConstPtr& msg)
-  {
-    cv_bridge::CvImagePtr cv_ptr;
-    try
-    {
-      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
-    }
-    catch (cv_bridge::Exception& e)
-    {
-      ROS_ERROR("cv_bridge exception: %s", e.what());
-      return;
-    }
-
-    // Draw an example circle on the video stream
-    if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110)
-      cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0));
-
-    // Update GUI Window
-    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
-    cv::waitKey(3);
-
-    // Output modified video stream
-    image_pub_.publish(cv_ptr->toCompressedImageMsg());
-  }
-};
-
-
-class SimpleCompressedExampleNodelet : public nodelet::Nodelet
-{
-
-
-public:
-  virtual void onInit()
-  {
-      simple_compressed_example::ImageConverter ic;
-      ros::spin();
-  }
-};
-
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(simple_compressed_example::SimpleCompressedExampleNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/simple_example_nodelet.cpp b/opencv_apps/src/nodelet/simple_example_nodelet.cpp
deleted file mode 100644
index 85f120d..0000000
--- a/opencv_apps/src/nodelet/simple_example_nodelet.cpp
+++ /dev/null
@@ -1,122 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2015, Tal Regev.
-*  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 Kei Okada 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.
-*********************************************************************/
-//http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
-/**
- * This is a demo of Simple Example from wiki tutorial
- */
-
-#include <ros/ros.h>
-#include <sensor_msgs/image_encodings.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-
-#include <dynamic_reconfigure/server.h>
-
-
-namespace simple_example {
-
-static const std::string OPENCV_WINDOW = "Image window";
-
-class ImageConverter
-{
-  ros::NodeHandle nh_;
-  image_transport::ImageTransport it_;
-  image_transport::Subscriber image_sub_;
-  image_transport::Publisher image_pub_;
-
-public:
-  ImageConverter()
-    : it_(nh_)
-  {
-    // Subscrive to input video feed and publish output video feed
-    image_sub_ = it_.subscribe("image", 1,
-      &ImageConverter::imageCb, this);
-    image_pub_ = it_.advertise("/image_converter/output_video/raw", 1);
-
-    cv::namedWindow(OPENCV_WINDOW);
-  }
-
-  ~ImageConverter()
-  {
-    cv::destroyWindow(OPENCV_WINDOW);
-  }
-
-  void imageCb(const sensor_msgs::ImageConstPtr& msg)
-  {
-    cv_bridge::CvImagePtr cv_ptr;
-    try
-    {
-      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
-    }
-    catch (cv_bridge::Exception& e)
-    {
-      ROS_ERROR("cv_bridge exception: %s", e.what());
-      return;
-    }
-
-    // Draw an example circle on the video stream
-    if (cv_ptr->image.rows > 110 && cv_ptr->image.cols > 110)
-      cv::circle(cv_ptr->image, cv::Point(cv_ptr->image.cols/2, cv_ptr->image.rows/2), 100, CV_RGB(255,0,0));
-
-    // Update GUI Window
-    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
-    cv::waitKey(3);
-
-    // Output modified video stream
-    image_pub_.publish(cv_ptr->toImageMsg());
-  }
-};
-
-
-class SimpleExampleNodelet : public nodelet::Nodelet
-{
-
-
-public:
-  virtual void onInit()
-  {
-      simple_example::ImageConverter ic;
-      ros::spin();
-  }
-};
-
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(simple_example::SimpleExampleNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/simple_flow_nodelet.cpp b/opencv_apps/src/nodelet/simple_flow_nodelet.cpp
deleted file mode 100644
index d454b2f..0000000
--- a/opencv_apps/src/nodelet/simple_flow_nodelet.cpp
+++ /dev/null
@@ -1,297 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/simpleflow_demo.cpp
-/**
- * This is a demo of SimpleFlow optical flow algorithm
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/video/tracking.hpp>
-#if CV_MAJOR_VERSION == 3
-#include <opencv2/optflow.hpp>
-#endif
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/SimpleFlowConfig.h"
-#include "opencv_apps/FlowArrayStamped.h"
-
-namespace simple_flow {
-class SimpleFlowNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  simple_flow::SimpleFlowConfig config_;
-  dynamic_reconfigure::Server<simple_flow::SimpleFlowConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_;
-  static bool need_config_update_;
-  int scale_;
-  
-  cv::Mat gray, prevGray;
-
-  void reconfigureCallback(simple_flow::SimpleFlowConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    scale_ = config_.scale;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame_src = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      /// Convert it to gray
-      cv::Mat frame;
-      if ( frame_src.channels() > 1 ) {
-        frame = frame_src;
-      } else {
-        cv::cvtColor( frame_src, frame, cv::COLOR_GRAY2BGR );
-      }
-
-      cv::resize(frame, gray, cv::Size(frame.cols/(double)MAX(1,scale_), frame.rows/(double)MAX(1,scale_)), 0, 0, CV_INTER_AREA);
-      if(prevGray.empty())
-        gray.copyTo(prevGray);
-
-      if (gray.rows != prevGray.rows && gray.cols != prevGray.cols) {
-        NODELET_WARN("Images should be of equal sizes");
-        gray.copyTo(prevGray);
-      }
-
-      if (frame.type() != 16) {
-        NODELET_ERROR("Images should be of equal type CV_8UC3");
-      }
-      // Messages
-      opencv_apps::FlowArrayStamped flows_msg;
-      flows_msg.header = msg->header;
-
-      // Do the work
-      cv::Mat flow;
-
-      if( debug_view_) {
-        cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
-        cv::createTrackbar( "Scale", window_name_, &scale_, 24, trackbarCallback);
-        if (need_config_update_) {
-          config_.scale = scale_;
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-      }
-
-      float start = (float)cv::getTickCount();
-#if CV_MAJOR_VERSION == 3
-      cv::optflow::calcOpticalFlowSF(gray, prevGray,
-#else
-      cv::calcOpticalFlowSF(gray, prevGray,
-#endif
-                            flow,
-                            3, 2, 4, 4.1, 25.5, 18, 55.0, 25.5, 0.35, 18, 55.0, 25.5, 10);
-      NODELET_INFO("calcOpticalFlowSF : %lf sec", (cv::getTickCount() - start) / cv::getTickFrequency());
-
-      //writeOpticalFlowToFile(flow, file);
-      int cols = flow.cols;
-      int rows = flow.rows;
-      double scale_col = frame.cols/(double)flow.cols;
-      double scale_row = frame.rows/(double)flow.rows;
-
-
-      for (int i= 0; i < rows; ++i) {
-        for (int j = 0; j < cols; ++j) {
-          cv::Vec2f flow_at_point = flow.at<cv::Vec2f>(i, j);
-          cv::line(frame, cv::Point(scale_col*j, scale_row*i), cv::Point(scale_col*(j+flow_at_point[0]), scale_row*(i+flow_at_point[1])), cv::Scalar(0,255,0), 1, 8, 0 );
-
-          opencv_apps::Flow flow_msg;
-          opencv_apps::Point2D point_msg;
-          opencv_apps::Point2D velocity_msg;
-          point_msg.x = scale_col*j;
-          point_msg.y = scale_row*i;
-          velocity_msg.x = scale_col*flow_at_point[0];
-          velocity_msg.y = scale_row*flow_at_point[1];
-          flow_msg.point = point_msg;
-          flow_msg.velocity = velocity_msg;
-          flows_msg.flow.push_back(flow_msg);
-        }
-      }
-
-      //cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
-      /// Apply Canny edge detector
-      //Canny( src_gray, edges, 50, 200, 3 );
-
-      //-- Show what you got
-      if ( debug_view_) {
-        cv::imshow( window_name_, frame );
-        int c = cv::waitKey(1);
-      }
-
-      cv::swap(prevGray, gray);
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(flows_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &SimpleFlowNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &SimpleFlowNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "simpleflow_demo";
-    scale_ = 4.0;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&SimpleFlowNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&SimpleFlowNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&SimpleFlowNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&SimpleFlowNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::FlowArrayStamped>("flows", 1, msg_connect_cb, msg_disconnect_cb);
-        
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<simple_flow::SimpleFlowConfig>::CallbackType f =
-      boost::bind(&SimpleFlowNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-  }
-};
-bool SimpleFlowNodelet::need_config_update_ = false;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(simple_flow::SimpleFlowNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp b/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp
deleted file mode 100644
index fc604b5..0000000
--- a/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp
+++ /dev/null
@@ -1,380 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-*  Copyright (c) 2014, Kei Okada.
-*  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 Kei Okada 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.
-*********************************************************************/
-
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/watershed.cpp
-/**
- * This program demonstrates the famous watershed segmentation algorithm in OpenCV: watershed()
- */
-
-#include <ros/ros.h>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include "opencv_apps/WatershedSegmentationConfig.h"
-#include "opencv_apps/Contour.h"
-#include "opencv_apps/ContourArray.h"
-#include "opencv_apps/ContourArrayStamped.h"
-#include "opencv_apps/Point2DArray.h"
-
-namespace watershed_segmentation {
-class WatershedSegmentationNodelet : public nodelet::Nodelet
-{
-  image_transport::Publisher img_pub_;
-  image_transport::Subscriber img_sub_;
-  image_transport::CameraSubscriber cam_sub_;
-  ros::Publisher msg_pub_;
-  ros::Subscriber add_seed_points_sub_;
-
-  boost::shared_ptr<image_transport::ImageTransport> it_;
-  ros::NodeHandle nh_, local_nh_;
-
-  watershed_segmentation::WatershedSegmentationConfig config_;
-  dynamic_reconfigure::Server<watershed_segmentation::WatershedSegmentationConfig> srv;
-
-  bool debug_view_;
-  int subscriber_count_;
-  ros::Time prev_stamp_;
-
-  std::string window_name_, segment_name_;
-  static bool need_config_update_;
-  static bool on_mouse_update_;
-  static int on_mouse_event_;
-  static int on_mouse_x_;
-  static int on_mouse_y_;
-  static int on_mouse_flags_;
-
-  cv::Mat markerMask;
-  cv::Point prevPt;
-
-  static void onMouse( int event, int x, int y, int flags, void* )
-  {
-    on_mouse_update_ = true;
-    on_mouse_event_ = event;
-    on_mouse_x_ = x;
-    on_mouse_y_ = y;
-    on_mouse_flags_ = flags;
-  }
-
-  void reconfigureCallback(watershed_segmentation::WatershedSegmentationConfig &new_config, uint32_t level)
-  {
-    config_ = new_config;
-    if (subscriber_count_)
-    { // @todo Could do this without an interruption at some point.
-      unsubscribe();
-      subscribe();
-    }
-  }
-
-  const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
-  {
-    if (frame.empty())
-      return image_frame;
-    return frame;
-  }
-
-  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
-  {
-    do_work(msg, cam_info->header.frame_id);
-  }
-  
-  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
-  {
-    do_work(msg, msg->header.frame_id);
-  }
-
-  static void trackbarCallback( int, void* )
-  {
-    need_config_update_ = true;
-  }
-
-  void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
-  {
-    // Work on the image.
-    try
-    {
-      // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
-
-      // Messages
-      opencv_apps::ContourArrayStamped contours_msg;
-      contours_msg.header = msg->header;
-
-      // Do the work
-      //std::vector<cv::Rect> faces;
-      cv::Mat imgGray;
-
-      /// Initialize
-      if ( markerMask.empty() )  {
-        cv::cvtColor(frame, markerMask, cv::COLOR_BGR2GRAY);
-        cv::cvtColor(markerMask, imgGray, cv::COLOR_GRAY2BGR);
-        markerMask = cv::Scalar::all(0);
-      }
-
-      if( debug_view_) {
-        cv::imshow( window_name_, frame);
-        cv::setMouseCallback( window_name_, onMouse, 0 );
-        if (need_config_update_) {
-          srv.updateConfig(config_);
-          need_config_update_ = false;
-        }
-
-        if ( on_mouse_update_ ) {
-          int event = on_mouse_event_;
-          int x = on_mouse_x_;
-          int y = on_mouse_y_;
-          int flags = on_mouse_flags_;
-
-          if( x < 0 || x >= frame.cols || y < 0 || y >= frame.rows )
-            return;
-          if( event == cv::EVENT_LBUTTONUP || !(flags & cv::EVENT_FLAG_LBUTTON) )
-            prevPt = cv::Point(-1,-1);
-          else if( event == cv::EVENT_LBUTTONDOWN )
-            prevPt = cv::Point(x,y);
-          else if( event == cv::EVENT_MOUSEMOVE && (flags & cv::EVENT_FLAG_LBUTTON) )
-          {
-            cv::Point pt(x, y);
-            if( prevPt.x < 0 )
-              prevPt = pt;
-            cv::line( markerMask, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 );
-            cv::line( frame, prevPt, pt, cv::Scalar::all(255), 5, 8, 0 );
-            prevPt = pt;
-            cv::imshow(window_name_, markerMask);
-          }
-        }
-        cv::waitKey(1);
-      }
-      
-      int i, j, compCount = 0;
-      std::vector<std::vector<cv::Point> > contours;
-      std::vector<cv::Vec4i> hierarchy;
-
-      cv::findContours(markerMask, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
-
-      if( contours.empty() ) {
-        NODELET_WARN("contnorus is empty");
-        return; //continue;
-      }
-      cv::Mat markers(markerMask.size(), CV_32S);
-      markers = cv::Scalar::all(0);
-      int idx = 0;
-      for( ; idx >= 0; idx = hierarchy[idx][0], compCount++ )
-        cv::drawContours(markers, contours, idx, cv::Scalar::all(compCount+1), -1, 8, hierarchy, INT_MAX);
-
-      if( compCount == 0 ) {
-        NODELET_WARN("compCount is 0");
-        return; //continue;
-      }
-      for( size_t i = 0; i< contours.size(); i++ ) {
-        opencv_apps::Contour contour_msg;
-        for ( size_t j = 0; j < contours[i].size(); j++ ) {
-          opencv_apps::Point2D point_msg;
-          point_msg.x = contours[i][j].x;
-          point_msg.y = contours[i][j].y;
-          contour_msg.points.push_back(point_msg);
-        }
-        contours_msg.contours.push_back(contour_msg);
-      }
-
-      std::vector<cv::Vec3b> colorTab;
-      for( i = 0; i < compCount; i++ )
-      {
-        int b = cv::theRNG().uniform(0, 255);
-        int g = cv::theRNG().uniform(0, 255);
-        int r = cv::theRNG().uniform(0, 255);
-
-        colorTab.push_back(cv::Vec3b((uchar)b, (uchar)g, (uchar)r));
-      }
-
-      double t = (double)cv::getTickCount();
-      cv::watershed( frame, markers );
-      t = (double)cv::getTickCount() - t;
-      NODELET_INFO( "execution time = %gms", t*1000./cv::getTickFrequency() );
-
-      cv::Mat wshed(markers.size(), CV_8UC3);
-
-      // paint the watershed image
-      for( i = 0; i < markers.rows; i++ )
-        for( j = 0; j < markers.cols; j++ )
-        {
-          int index = markers.at<int>(i,j);
-          if( index == -1 )
-            wshed.at<cv::Vec3b>(i,j) = cv::Vec3b(255,255,255);
-          else if( index <= 0 || index > compCount )
-            wshed.at<cv::Vec3b>(i,j) = cv::Vec3b(0,0,0);
-          else
-            wshed.at<cv::Vec3b>(i,j) = colorTab[index - 1];
-        }
-
-      wshed = wshed*0.5 + imgGray*0.5;
-
-      //-- Show what you got
-      if( debug_view_) {
-        cv::imshow( segment_name_, wshed );
-
-        int c = cv::waitKey(1);
-        //if( (char)c == 27 )
-        //    break;
-        if( (char)c == 'r' )
-        {
-          markerMask = cv::Scalar::all(0);
-        }
-      }
-
-      // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, wshed).toImageMsg();
-      img_pub_.publish(out_img);
-      msg_pub_.publish(contours_msg);
-    }
-    catch (cv::Exception &e)
-    {
-      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-    }
-
-    prev_stamp_ = msg->header.stamp;
-  }
-
-  void add_seed_point_cb(const opencv_apps::Point2DArray& msg) {
-    if ( msg.points.size() == 0 ) {
-      markerMask = cv::Scalar::all(0);
-    } else {
-      for(size_t i = 0; i < msg.points.size(); i++ ) {
-        cv::Point pt0(msg.points[i].x, msg.points[i].y);
-        cv::Point pt1(pt0.x + 1, pt0.y + 1);
-        cv::line( markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0 );
-      }
-    }
-  }
-
-  void subscribe()
-  {
-    NODELET_DEBUG("Subscribing to image topic.");
-    if (config_.use_camera_info)
-      cam_sub_ = it_->subscribeCamera("image", 3, &WatershedSegmentationNodelet::imageCallbackWithInfo, this);
-    else
-      img_sub_ = it_->subscribe("image", 3, &WatershedSegmentationNodelet::imageCallback, this);
-  }
-
-  void unsubscribe()
-  {
-    NODELET_DEBUG("Unsubscribing from image topic.");
-    img_sub_.shutdown();
-    cam_sub_.shutdown();
-  }
-
-  void img_connectCb(const image_transport::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void img_disconnectCb(const image_transport::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-  void msg_connectCb(const ros::SingleSubscriberPublisher& ssp)
-  {
-    if (subscriber_count_++ == 0) {
-      subscribe();
-    }
-  }
-
-  void msg_disconnectCb(const ros::SingleSubscriberPublisher&)
-  {
-    subscriber_count_--;
-    if (subscriber_count_ == 0) {
-      unsubscribe();
-    }
-  }
-
-public:
-  virtual void onInit()
-  {
-    nh_ = getNodeHandle();
-    it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
-    local_nh_ = ros::NodeHandle("~");
-
-    local_nh_.param("debug_view", debug_view_, false);
-    subscriber_count_ = 0;
-    prev_stamp_ = ros::Time(0, 0);
-
-    window_name_ = "roughly mark the areas to segment on the image";
-    segment_name_ = "watershed transform";
-    prevPt.x = -1;
-    prevPt.y = -1;
-
-    image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&WatershedSegmentationNodelet::img_connectCb, this, _1);
-    image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&WatershedSegmentationNodelet::img_disconnectCb, this, _1);
-    ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&WatershedSegmentationNodelet::msg_connectCb, this, _1);
-    ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&WatershedSegmentationNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
-    add_seed_points_sub_ = local_nh_.subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this);
-        
-    if( debug_view_ ) {
-      subscriber_count_++;
-    }
-
-    dynamic_reconfigure::Server<watershed_segmentation::WatershedSegmentationConfig>::CallbackType f =
-      boost::bind(&WatershedSegmentationNodelet::reconfigureCallback, this, _1, _2);
-    srv.setCallback(f);
-
-    NODELET_INFO("This program demonstrates the famous watershed segmentation algorithm in OpenCV: watershed()");
-    NODELET_INFO("Hot keys: ");
-    NODELET_INFO("\tESC - quit the program");
-    NODELET_INFO("\tr - restore the original image");
-    NODELET_INFO("\tw or SPACE - run watershed segmentation algorithm");
-    NODELET_INFO("\t\t(before running it, *roughly* mark the areas to segment on the image)");
-    NODELET_INFO("\t  (before that, roughly outline several markers on the image)");
-  }
-};
-bool WatershedSegmentationNodelet::need_config_update_ = false;
-bool WatershedSegmentationNodelet::on_mouse_update_ = false;
-int WatershedSegmentationNodelet::on_mouse_event_ = 0;
-int WatershedSegmentationNodelet::on_mouse_x_ = 0;
-int WatershedSegmentationNodelet::on_mouse_y_ = 0;
-int WatershedSegmentationNodelet::on_mouse_flags_ = 0;
-}
-
-#include <pluginlib/class_list_macros.h>
-PLUGINLIB_EXPORT_CLASS(watershed_segmentation::WatershedSegmentationNodelet, nodelet::Nodelet);
diff --git a/opencv_apps/test/CMakeLists.txt b/opencv_apps/test/CMakeLists.txt
deleted file mode 100644
index dbfa7e8..0000000
--- a/opencv_apps/test/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-# Tests simple calibration dataset
-catkin_download_test_data(face_detector_withface_test_diamondback.bag http://download.ros.org/data/face_detector/face_detector_withface_test_diamondback.bag
-  DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}
-  MD5 59126117e049e69d577b7ee27251a6f8
-  )
-catkin_download_test_data(vslam_tutorial.bag http://download.ros.org/data/vslam_system/vslam_tutorial.bag
-  DESTINATION .
-  MD5 f5aece448b7af00a38a993eb71400806
-  )
-add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag
-  DEPENDS vslam_tutorial.bag
-  COMMAND rosbag reindex vslam_tutorial.bag --output-dir ${CMAKE_CURRENT_SOURCE_DIR}
-)
-add_custom_target(vslam_tutorial_bag DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag)
-add_dependencies(tests vslam_tutorial_bag)
-
-#add_rostest(test-apps.test)
-add_rostest(test-edge_detection.test ARGS gui:=false)
-add_rostest(test-hough_lines.test ARGS gui:=false)
-add_rostest(test-hough_circles.test ARGS gui:=false)
-add_rostest(test-find_contours.test ARGS gui:=false)
-add_rostest(test-convex_hull.test ARGS gui:=false)
-add_rostest(test-general_contours.test ARGS gui:=false)
-add_rostest(test-contour_moments.test ARGS gui:=false)
-add_rostest(test-face_detection.test ARGS gui:=false)
-add_rostest(test-goodfeature_track.test ARGS gui:=false)
-add_rostest(test-camshift.test ARGS gui:=false)
-add_rostest(test-fback_flow.test ARGS gui:=false)
-add_rostest(test-lk_flow.test ARGS gui:=false)
-add_rostest(test-people_detect.test ARGS gui:=false)
-add_rostest(test-phase_corr.test ARGS gui:=false)
-add_rostest(test-segment_objects.test ARGS gui:=false)
-# simple flow requires opencv-contrib https://github.com/ros-perception/vision_opencv/issues/108
-if(OPENCV_HAVE_OPTFLOW)
-  add_rostest(test-simple_flow.test ARGS gui:=false)
-endif()
-add_rostest(test-watershed_segmentation.test ARGS gui:=false)
-
-
diff --git a/opencv_apps/test/test-camshift.test b/opencv_apps/test/test-camshift.test
deleted file mode 100644
index be21796..0000000
--- a/opencv_apps/test/test-camshift.test
+++ /dev/null
@@ -1,32 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- camshift.cpp -->
-    <node name="camshift" pkg="opencv_apps" type="camshift" args="image:=image_rect_color" >
-      <param name="debug_view" value="$(arg gui)" />
-      <rosparam param="histogram">
-        [0.0, 255.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-      </rosparam>
-      <param name="vmin" value="120" />
-      <param name="vmax" value="230" />
-      <param name="smin" value="60" />
-    </node>
-    <node name="camshift_saver_result" pkg="image_view" type="image_saver" args="image:=camshift/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/camshift_result.png"/>
-    </node>
-    <node name="camshift_saver_back_project" pkg="image_view" type="image_saver" args="image:=camshift/back_project" >
-      <param name="filename_format" value="$(find opencv_apps)/test/camshift_back_project.png"/>
-    </node>
-    <param name="camshift_test/topic" value="camshift/track_box" />    <!-- opencv_apps/Point2DArrayStamped -->
-    <test test-name="camshift_test" pkg="rostest" type="hztest" name="camshift_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-contour_moments.test b/opencv_apps/test/test-contour_moments.test
deleted file mode 100644
index fcd9d71..0000000
--- a/opencv_apps/test/test-contour_moments.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- contour_moments.cpp -->
-    <node name="contour_moments" pkg="opencv_apps" type="contour_moments" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="threshold" value="30" />
-    </node>
-    <node name="contour_moments_saver" pkg="image_view" type="image_saver" args="image:=contour_moments/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/contour_moments.png"/>
-    </node>
-    <param name="contour_moments_test/topic" value="contour_moments/moments" />    <!-- opencv_apps/MomentArrayStamped -->
-    <test test-name="contour_moments_test" pkg="rostest" type="hztest" name="contour_moments_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-convex_hull.test b/opencv_apps/test/test-convex_hull.test
deleted file mode 100644
index 0bae9b1..0000000
--- a/opencv_apps/test/test-convex_hull.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- convex_hull.cpp -->
-    <node name="convex_hull" pkg="opencv_apps" type="convex_hull" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="threshold" value="20" />
-    </node>
-    <node name="convex_hull_saver" pkg="image_view" type="image_saver" args="image:=convex_hull/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/convex_hull.png"/>
-    </node>
-    <param name="convex_hull_test/topic" value="convex_hull/hulls" />    <!-- opencv_apps/ContorArrayStamped -->
-    <test test-name="convex_hull_test" pkg="rostest" type="hztest" name="convex_hull_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-edge_detection.test b/opencv_apps/test/test-edge_detection.test
deleted file mode 100644
index 0a84388..0000000
--- a/opencv_apps/test/test-edge_detection.test
+++ /dev/null
@@ -1,54 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- edge_detection.cpp  -->
-    <!-- 0: Sobel -->
-    <node name="edge_sobel_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="edge_type" value="0" />
-    </node>
-    <node name="edge_sobel_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_sobel_detection/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/edge_sobel_detection.png"/>
-    </node>
-    <param name="edge_sobel_detection_test/topic" value="edge_sobel_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
-    <test test-name="edge_sobel_detection_test" pkg="rostest" type="hztest" name="edge_sobel_detection_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-    <!-- 1: Laplace -->
-    <node name="edge_laplace_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="edge_type" value="1" />
-    </node>
-    <node name="edge_laplace_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_laplace_detection/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/edge_laplace_detection.png"/>
-    </node>
-    <param name="edge_laplace_detection_test/topic" value="edge_laplace_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
-    <test test-name="edge_laplace_detection_test" pkg="rostest" type="hztest" name="edge_laplace_detection_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-    <!-- 2: Canny -->
-    <node name="edge_canny_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="edge_type" value="2" />
-      <param name="canny_low_threshold" value="40" />
-    </node>
-    <node name="edge_canny_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_canny_detection/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/edge_canny_detection.png"/>
-    </node>
-    <param name="edge_canny_detection_test/topic" value="edge_canny_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
-    <test test-name="edge_canny_detection_test" pkg="rostest" type="hztest" name="edge_canny_detection_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-face_detection.test b/opencv_apps/test/test-face_detection.test
deleted file mode 100644
index 5d84a3f..0000000
--- a/opencv_apps/test/test-face_detection.test
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- face_detection.cpp -->
-    <node name="face_detection" pkg="opencv_apps" type="face_detection" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="face_detection_saver" pkg="image_view" type="image_saver" args="image:=face_detection/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/face_detection.png"/>
-    </node>
-    <param name="face_detection_test/topic" value="face_detection/faces" />    <!-- opencv_apps/FaceArrayStamped -->
-    <test test-name="face_detection_test" pkg="rostest" type="hztest" name="face_detection_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-fback_flow.test b/opencv_apps/test/test-fback_flow.test
deleted file mode 100644
index 398c51e..0000000
--- a/opencv_apps/test/test-fback_flow.test
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
-
-  <group ns="narrow_stereo/left" >
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
-
-    <!-- fback_flow.cpp -->
-    <node name="fback_flow" pkg="opencv_apps" type="fback_flow" args="image:=image_rect" >
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="fback_flow_saver" pkg="image_view" type="image_saver" args="image:=fback_flow/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/fback_flow.png"/>
-    </node>
-    <param name="fback_flow_test/topic" value="fback_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
-    <test test-name="fback_flow_test" pkg="rostest" type="hztest" name="fback_flow_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-find_contours.test b/opencv_apps/test/test-find_contours.test
deleted file mode 100644
index 68b4bca..0000000
--- a/opencv_apps/test/test-find_contours.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- find_contours.cpp -->
-    <node name="find_contours" pkg="opencv_apps" type="find_contours" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="canny_low_threshold" value="100" />
-    </node>
-    <node name="find_contours_saver" pkg="image_view" type="image_saver" args="image:=find_contours/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/find_contours.png"/>
-    </node>
-    <param name="find_contours_test/topic" value="find_contours/contours" />    <!-- opencv_apps/ContorArrayStamped -->
-    <test test-name="find_contours_test" pkg="rostest" type="hztest" name="find_contours_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-general_contours.test b/opencv_apps/test/test-general_contours.test
deleted file mode 100644
index 2466f18..0000000
--- a/opencv_apps/test/test-general_contours.test
+++ /dev/null
@@ -1,30 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- general_contours.cpp -->
-    <node name="general_contours" pkg="opencv_apps" type="general_contours" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="threshold" value="30" />
-    </node>
-    <node name="general_contours_saver" pkg="image_view" type="image_saver" args="image:=general_contours/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/general_contours.png"/>
-    </node>
-    <param name="general_contours_rectangles_test/topic" value="general_contours/rectangles" />    <!-- opencv_apps/RotatedRectArrayStamped -->
-    <test test-name="general_contours_rectangles_test" pkg="rostest" type="hztest" name="general_contours_rectangles_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-    <param name="general_contours_test_ellipses/topic" value="general_contours/ellipses" />    <!-- opencv_apps/RotatedRectArrayStamped -->
-    <test test-name="general_contours_ellipses_test" pkg="rostest" type="hztest" name="general_contours_test_ellipses" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-goodfeature_track.test b/opencv_apps/test/test-goodfeature_track.test
deleted file mode 100644
index 14d363d..0000000
--- a/opencv_apps/test/test-goodfeature_track.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- goodfeature_track.cpp -->
-    <node name="goodfeature_track" pkg="opencv_apps" type="goodfeature_track" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="max_corners" value="50" />
-    </node>
-    <node name="goodfeature_track_saver" pkg="image_view" type="image_saver" args="image:=goodfeature_track/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/goodfeature_track.png"/>
-    </node>
-    <param name="goodfeature_track_test/topic" value="goodfeature_track/corners" />    <!-- opencv_apps/Point2DArrayStamped -->
-    <test test-name="goodfeature_track_test" pkg="rostest" type="hztest" name="goodfeature_track_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-hough_circles.test b/opencv_apps/test/test-hough_circles.test
deleted file mode 100644
index 886f800..0000000
--- a/opencv_apps/test/test-hough_circles.test
+++ /dev/null
@@ -1,25 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- hough_circles.cpp -->
-    <node name="hough_circles" pkg="opencv_apps" type="hough_circles" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="canny_threshold" value="200" />
-      <param name="accumulator_threshold" value="20" />
-    </node>
-    <node name="hough_circles_saver" pkg="image_view" type="image_saver" args="image:=hough_circles/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/hough_circles.png"/>
-    </node>
-    <param name="hough_circles_test/topic" value="hough_circles/circles" />    <!-- opencv_apps/CircleArrayStamped -->
-    <test test-name="hough_circles_test" pkg="rostest" type="hztest" name="hough_circles_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-hough_lines.test b/opencv_apps/test/test-hough_lines.test
deleted file mode 100644
index f5fc8b9..0000000
--- a/opencv_apps/test/test-hough_lines.test
+++ /dev/null
@@ -1,42 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- hough_lines.cpp -->
-    <!-- 0: Standard Hough Line -->
-    <node name="standard_hough_lines" pkg="opencv_apps" type="hough_lines" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="hough_type" value="0" />
-      <param name="threshold" value="100" />
-    </node>
-    <node name="standard_hough_lines_saver" pkg="image_view" type="image_saver" args="image:=standard_hough_lines/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/standard_hough_lines.png"/>
-    </node>
-    <param name="standard_hough_lines_test/topic" value="standard_hough_lines/lines" />    <!-- opencv_apps/LineArrayStamped -->
-    <test test-name="standard_hough_lines_test" pkg="rostest" type="hztest" name="standard_hough_lines_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-
-    <!-- 1: Probabilistic Hough Line -->
-    <node name="probabilistic_hough_lines" pkg="opencv_apps" type="hough_lines" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />e
-      <param name="hough_type" value="1" />
-      <param name="threshold" value="100" />
-    </node>
-    <node name="probabilistic_hough_lines_saver" pkg="image_view" type="image_saver" args="image:=probabilistic_hough_lines/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/probabilistic_hough_lines.png"/>
-    </node>
-    <param name="probabilistic_hough_lines_test/topic" value="probabilistic_hough_lines/lines" />    <!-- opencv_apps/LineArrayStamped -->
-    <test test-name="probabilistic_hough_lines_test" pkg="rostest" type="hztest" name="probabilistic_hough_lines_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-lk_flow.test b/opencv_apps/test/test-lk_flow.test
deleted file mode 100644
index 8c28e23..0000000
--- a/opencv_apps/test/test-lk_flow.test
+++ /dev/null
@@ -1,25 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
-
-  <group ns="narrow_stereo/left" >
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_throttle" if="$(arg gui)" />
-    <node name="image_throttle" pkg="topic_tools" type="throttle" args="messages image_rect 5" />
-    <!-- lk_flow.cpp -->
-    <node name="lk_flow" pkg="opencv_apps" type="lk_flow" args="image:=image_rect_throttle" >
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="lk_flow_saver" pkg="image_view" type="image_saver" args="image:=lk_flow/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/lk_flow.png"/>
-    </node>
-    <node name="lk_flow_initialize" pkg="rosservice" type="rosservice" args="call --wait lk_flow/initialize_points"
-          respawn="true" respawn_delay="2" />
-    <param name="lk_flow_test/topic" value="lk_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
-    <test test-name="lk_flow_test" pkg="rostest" type="hztest" name="lk_flow_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-people_detect.test b/opencv_apps/test/test-people_detect.test
deleted file mode 100644
index b8ad3a5..0000000
--- a/opencv_apps/test/test-people_detect.test
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- people_detect.cpp -->
-    <node name="people_detect" pkg="opencv_apps" type="people_detect" args="image:=image_rect" >
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="people_detect_saver" pkg="image_view" type="image_saver" args="image:=people_detect/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/people_detect.png"/>
-    </node>
-    <param name="people_detect_test/topic" value="people_detect/found" />    <!-- opencv_apps/RectArrayStamped -->
-    <test test-name="people_detect_test" pkg="rostest" type="hztest" name="people_detect_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-phase_corr.test b/opencv_apps/test/test-phase_corr.test
deleted file mode 100644
index 83c1c8d..0000000
--- a/opencv_apps/test/test-phase_corr.test
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
-
-  <group ns="narrow_stereo/left" >
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
-
-    <!-- phase_corr.cpp -->
-    <node name="phase_corr" pkg="opencv_apps" type="phase_corr" args="image:=image_rect" >
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="phase_corr_saver" pkg="image_view" type="image_saver" args="image:=phase_corr/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/phase_corr.png"/>
-    </node>
-    <param name="phase_corr_test/topic" value="phase_corr/shift" />    <!-- opencv_apps/Point2DStamped -->
-    <test test-name="phase_corr_test" pkg="rostest" type="hztest" name="phase_corr_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-segment_objects.test b/opencv_apps/test/test-segment_objects.test
deleted file mode 100644
index 1c61059..0000000
--- a/opencv_apps/test/test-segment_objects.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- segment_objects.cpp -->
-    <node name="segment_objects" pkg="opencv_apps" type="segment_objects" args="image:=image_rect" >
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="segment_objects_saver" pkg="image_view" type="image_saver" args="image:=segment_objects/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/segment_objects.png"/>
-    </node>
-    <param name="segment_objects_test/topic" value="segment_objects/contours" />    <!-- opencv_apps/ContoruArrayStamped -->
-    <test test-name="segment_objects_test" pkg="rostest" type="hztest" name="segment_objects_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-simple_flow.test b/opencv_apps/test/test-simple_flow.test
deleted file mode 100644
index 25b05ee..0000000
--- a/opencv_apps/test/test-simple_flow.test
+++ /dev/null
@@ -1,24 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
-
-  <group ns="narrow_stereo/left" >
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
-
-    <!-- simple_flow.cpp -->
-    <node name="simple_flow" pkg="opencv_apps" type="simple_flow" args="image:=image_rect" >
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-      <param name="scale" value="8" />
-    </node>
-    <node name="simple_flow_saver" pkg="image_view" type="image_saver" args="image:=simple_flow/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/simple_flow.png"/>
-    </node>
-    <param name="simple_flow_test/topic" value="simple_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
-    <test test-name="simple_flow_test" pkg="rostest" type="hztest" name="simple_flow_test" >
-      <param name="hz" value="15" />
-      <param name="hzerror" value="14" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_apps/test/test-watershed_segmentation.test b/opencv_apps/test/test-watershed_segmentation.test
deleted file mode 100644
index 0d42635..0000000
--- a/opencv_apps/test/test-watershed_segmentation.test
+++ /dev/null
@@ -1,26 +0,0 @@
-<launch>
-  <arg name="gui" default="true" />
-  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
-
-  <group ns="wide_stereo/left" >
-    <node name="image_proc" pkg="image_proc" type="image_proc" />
-    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
-
-    <!-- watershed_segmentation -->
-    <node name="watershed_segmentation" pkg="opencv_apps" type="watershed_segmentation" args="image:=image_rect_color" output="screen">
-      <param name="use_camera_info" value="false" />
-      <param name="debug_view" value="$(arg gui)" />
-    </node>
-    <node name="pub_add_seed_points" pkg="rostopic" type="rostopic"
-            args="pub -r 1 watershed_segmentation/add_seed_points opencv_apps/Point2DArray '[{x: 100, y: 100},{x: 200, y: 100},{x: 300, y: 100},{x: 400, y: 100},{x: 500, y: 100},{x: 100, y: 200},{x: 200, y: 200},{x: 300, y: 200},{x: 400, y: 200},{x: 500, y: 200},{x: 100, y: 300},{x: 200, y: 300},{x: 300, y: 300},{x: 400, y: 300},{x: 500, y: 300}]' --" />
-    <node name="watershed_segmentation_saver" pkg="image_view" type="image_saver" args="image:=watershed_segmentation/image" >
-      <param name="filename_format" value="$(find opencv_apps)/test/watershed_segmentation.png"/>
-    </node>
-    <param name="watershed_segmentation_test/topic" value="watershed_segmentation/contours" />    <!-- opencv_apps/ContourStamped -->
-    <test test-name="watershed_segmentation_test" pkg="rostest" type="hztest" name="watershed_segmentation_test" >
-      <param name="hz" value="20" />
-      <param name="hzerror" value="15" />
-      <param name="test_duration" value="5.0" /> 
-    </test>
-  </group>
-</launch>
diff --git a/opencv_tests/CHANGELOG.rst b/opencv_tests/CHANGELOG.rst
index 899ea96..52ce2c8 100644
--- a/opencv_tests/CHANGELOG.rst
+++ b/opencv_tests/CHANGELOG.rst
@@ -2,6 +2,47 @@
 Changelog for package opencv_tests
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.12.2 (2016-09-24)
+-------------------
+
+1.12.1 (2016-07-11)
+-------------------
+* Support compressed Images messages in python for indigo
+  - Add cv2_to_comprssed_imgmsg: Convert from cv2 image to compressed image ros msg.
+  - Add comprssed_imgmsg_to_cv2:   Convert the compress message to a new image.
+  - Add compressed image tests.
+  - Add time to msgs (compressed and regular).
+  add enumerants test for compressed image.
+  merge the compressed tests with the regular ones.
+  better comment explanation. I will squash this commit.
+  Fix indentation
+  fix typo mistage: from .imgmsg_to_compressed_cv2 to .compressed_imgmsg_to_cv2.
+  remove cv2.CV_8UC1
+  remove rospy and time depndency.
+  change from IMREAD_COLOR to IMREAD_ANYCOLOR.
+  - make indentaion of 4.
+  - remove space trailer.
+  - remove space from empty lines.
+  - another set of for loops, it will make things easier to track. In that new set,  just have the number of channels in ([],1,3,4) (ignore two for jpg). from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721943
+  - keep the OpenCV error message. from: https://github.com/ros-perception/vision_opencv/pull/132#discussion_r66721013
+  add debug print for test.
+  add case for 4 channels in test.
+  remove 4 channels case from compressed test.
+  add debug print for test.
+  change typo of format.
+  fix typo in format. change from dip to dib.
+  change to IMREAD_ANYCOLOR as python code. (as it should).
+  rename TIFF to tiff
+  Sperate the tests one for regular images and one for compressed.
+  update comment
+* Contributors: talregev
+
+1.12.0 (2016-03-18)
+-------------------
+
+1.11.12 (2016-03-10)
+--------------------
+
 1.11.11 (2016-01-31)
 --------------------
 * fix a few warnings in doc jobs
diff --git a/opencv_tests/nodes/broadcast.py b/opencv_tests/nodes/broadcast.py
index 831e681..0df824d 100755
--- a/opencv_tests/nodes/broadcast.py
+++ b/opencv_tests/nodes/broadcast.py
@@ -2,6 +2,7 @@
 # Software License Agreement (BSD License)
 #
 # Copyright (c) 2008, Willow Garage, Inc.
+# Copyright (c) 2016, Tal Regev.
 # All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
@@ -40,32 +41,38 @@ import cv2
 import sensor_msgs.msg
 from cv_bridge import CvBridge
 
-class source:
 
-  def __init__(self, topic, filenames):
-    self.pub = rospy.Publisher(topic, sensor_msgs.msg.Image)
-    self.filenames = filenames
+# Send each image by iterate it from given array of files names  to a given topic,
+# as a regular and compressed ROS Images msgs.
+class Source:
+
+    def __init__(self, topic, filenames):
+        self.pub            = rospy.Publisher(topic, sensor_msgs.msg.Image)
+        self.pub_compressed = rospy.Publisher(topic + "/compressed", sensor_msgs.msg.CompressedImage)
+        self.filenames      = filenames
+
+    def spin(self):
+        time.sleep(1.0)
+        cvb = CvBridge()
+        while not rospy.core.is_shutdown():
+            cvim = cv2.imload(self.filenames[0])
+            self.pub.publish(cvb.cv2_to_imgmsg(cvim))
+            self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
+            self.filenames = self.filenames[1:] + [self.filenames[0]]
+            time.sleep(1)
 
-  def spin(self):
-    time.sleep(1.0)
-    cvb = CvBridge()
-    while not rospy.core.is_shutdown():
-      cvim = cv2.imload(self.filenames[0])
-      self.pub.publish(cvb.cv2_to_imgmsg(cvim))
-      self.filenames = self.filenames[1:] + [self.filenames[0]]
-      time.sleep(1)
 
 def main(args):
-  s = source(args[1], args[2:])
-  rospy.init_node('source')
-  try:
-    s.spin()
-    rospy.spin()
-    outcome = 'test completed'
-  except KeyboardInterrupt:
-    print "shutting down"
-    outcome = 'keyboard interrupt'
-  rospy.core.signal_shutdown(outcome)
+    s = Source(args[1], args[2:])
+    rospy.init_node('Source')
+    try:
+        s.spin()
+        rospy.spin()
+        outcome = 'test completed'
+    except KeyboardInterrupt:
+        print "shutting down"
+        outcome = 'keyboard interrupt'
+    rospy.core.signal_shutdown(outcome)
 
 if __name__ == '__main__':
-  main(sys.argv)
+    main(sys.argv)
diff --git a/opencv_tests/nodes/rosfacedetect.py b/opencv_tests/nodes/rosfacedetect.py
index 6d137e2..9b072d7 100755
--- a/opencv_tests/nodes/rosfacedetect.py
+++ b/opencv_tests/nodes/rosfacedetect.py
@@ -5,6 +5,7 @@ The program finds faces in a camera image or video stream and displays a red box
 
 Original C implementation by:  ?
 Python implementation by: Roman Stanchak, James Bowman
+Updated: Copyright (c) 2016, Tal Regev.
 """
 
 import sys
@@ -19,25 +20,27 @@ import numpy
 
 # Parameters for haar detection
 # From the API:
-# The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned 
-# for accurate yet slow object detection. For a faster operation on real video 
-# images the settings are: 
-# scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING, 
+# The default parameters (scale_factor=2, min_neighbors=3, flags=0) are tuned
+# for accurate yet slow object detection. For a faster operation on real video
+# images the settings are:
+# scale_factor=1.2, min_neighbors=2, flags=CV_HAAR_DO_CANNY_PRUNING,
 # min_size=<minimum possible face size
 
-min_size = (10, 10)
-image_scale = 2
-haar_scale = 1.2
+min_size      = (10, 10)
+image_scale   = 2
+haar_scale    = 1.2
 min_neighbors = 2
-haar_flags = 0
+haar_flags    = 0
 
 if __name__ == '__main__':
 
+    # TODO add this file in the repository and make it relative to this python script. (not all people will run this from linux)
     haarfile = '/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml'
 
     parser = OptionParser(usage = "usage: %prog [options]")
     parser.add_option("-c", "--cascade", action="store", dest="cascade", type="str", help="Haar cascade file, default %default", default = haarfile)
     parser.add_option("-t", "--topic", action="store", dest="topic", type="str", help="Topic to find a face on, default %default", default = '/camera/rgb/image_raw')
+    parser.add_option("-ct", "--ctopic", action="store", dest="ctopic", type="str", help="Compressed topic to find a face on, default %default", default = '/camera/rgb/image/compressed')
     (options, args) = parser.parse_args()
 
     cascade = cv2.CascadeClassifier()
@@ -61,7 +64,7 @@ if __name__ == '__main__':
             faces = cascade.detectMultiScale(small_img, haar_scale, min_neighbors, haar_flags, min_size)
             if faces is not None:
                 for (x, y, w, h) in faces:
-                    # the input to detectMultiScale was resized, so scale the 
+                    # the input to detectMultiScale was resized, so scale the
                     # bounding box of each face and convert it to two CvPoints
                     pt1 = (int(x * image_scale), int(y * image_scale))
                     pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
@@ -70,6 +73,33 @@ if __name__ == '__main__':
         cv2.imshow("result", img)
         cv2.waitKey(6)
 
+    def compressed_detect_and_draw(compressed_imgmsg):
+        img = br.compressed_imgmsg_to_cv2(compressed_imgmsg, "bgr8")
+        # allocate temporary images
+        new_size = (int(img.shape[1] / image_scale), int(img.shape[0] / image_scale))
+
+        # convert color input image to grayscale
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+
+        # scale input image for faster processing
+        small_img = cv2.resize(gray, new_size, interpolation = cv2.INTER_LINEAR)
+
+        small_img = cv2.equalizeHist(small_img)
+
+        if(cascade):
+            faces = cascade.detectMultiScale(small_img, haar_scale, min_neighbors, haar_flags, min_size)
+            if faces is not None:
+                for (x, y, w, h) in faces:
+                    # the input to detectMultiScale was resized, so scale the
+                    # bounding box of each face and convert it to two CvPoints
+                    pt1 = (int(x * image_scale), int(y * image_scale))
+                    pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
+                    cv2.rectangle(img, pt1, pt2, (255, 0, 0), 3, 8, 0)
+
+        cv2.imshow("compressed_result", img)
+        cv2.waitKey(6)
+
     rospy.init_node('rosfacedetect')
     rospy.Subscriber(options.topic, sensor_msgs.msg.Image, detect_and_draw)
+    rospy.Subscriber(options.ctopic, sensor_msgs.msg.CompressedImage, compressed_detect_and_draw)
     rospy.spin()
diff --git a/opencv_tests/nodes/source.py b/opencv_tests/nodes/source.py
index 8676391..4662fbe 100755
--- a/opencv_tests/nodes/source.py
+++ b/opencv_tests/nodes/source.py
@@ -2,6 +2,7 @@
 # Software License Agreement (BSD License)
 #
 # Copyright (c) 2008, Willow Garage, Inc.
+# Copyright (c) 2016, Tal Regev.
 # All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
@@ -41,50 +42,54 @@ import cv2
 import sensor_msgs.msg
 from cv_bridge import CvBridge
 
-class source:
 
-  def __init__(self):
-    self.pub = rospy.Publisher("/opencv_tests/images", sensor_msgs.msg.Image)
+# Send black pic with a circle as regular and compressed ros msgs.
+class Source:
 
-  def spin(self):
-    time.sleep(1.0)
-    started = time.time()
-    counter = 0
-    cvim = numpy.zeros((480, 640, 1), numpy.uint8)
-    ball_xv = 10
-    ball_yv = 10
-    ball_x = 100
-    ball_y = 100
+    def __init__(self):
+        self.pub            = rospy.Publisher("/opencv_tests/images", sensor_msgs.msg.Image)
+        self.pub_compressed = rospy.Publisher("/opencv_tests/images/compressed", sensor_msgs.msg.CompressedImage)
 
-    cvb = CvBridge()
+    def spin(self):
+        time.sleep(1.0)
+        started = time.time()
+        counter = 0
+        cvim = numpy.zeros((480, 640, 1), numpy.uint8)
+        ball_xv = 10
+        ball_yv = 10
+        ball_x = 100
+        ball_y = 100
 
-    while not rospy.core.is_shutdown():
+        cvb = CvBridge()
 
-      cvim.fill(0)
-      cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
+        while not rospy.core.is_shutdown():
 
-      ball_x += ball_xv
-      ball_y += ball_yv
-      if ball_x in [10, 630]:
-        ball_xv = -ball_xv
-      if ball_y in [10, 470]:
-        ball_yv = -ball_yv
+            cvim.fill(0)
+            cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1)
 
-      self.pub.publish(cvb.cv2_to_imgmsg(cvim))
+            ball_x += ball_xv
+            ball_y += ball_yv
+            if ball_x in [10, 630]:
+                ball_xv = -ball_xv
+            if ball_y in [10, 470]:
+                ball_yv = -ball_yv
+
+            self.pub.publish(cvb.cv2_to_imgmsg(cvim))
+            self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim))
+            time.sleep(0.03)
 
-      time.sleep(0.03)
 
 def main(args):
-  s = source()
-  rospy.init_node('source')
-  try:
-    s.spin()
-    rospy.spin()
-    outcome = 'test completed'
-  except KeyboardInterrupt:
-    print "shutting down"
-    outcome = 'keyboard interrupt'
-  rospy.core.signal_shutdown(outcome)
+    s = Source()
+    rospy.init_node('Source')
+    try:
+        s.spin()
+        rospy.spin()
+        outcome = 'test completed'
+    except KeyboardInterrupt:
+        print "shutting down"
+        outcome = 'keyboard interrupt'
+    rospy.core.signal_shutdown(outcome)
 
 if __name__ == '__main__':
-  main(sys.argv)
+    main(sys.argv)
diff --git a/opencv_tests/package.xml b/opencv_tests/package.xml
index 4cc2841..472932d 100644
--- a/opencv_tests/package.xml
+++ b/opencv_tests/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>opencv_tests</name>
-  <version>1.11.11</version>
+  <version>1.12.2</version>
   <description>
     Tests the enumerants of the ROS Image message, and functionally tests the Python and C++ implementations of CvBridge.
   </description>
diff --git a/vision_opencv/CHANGELOG.rst b/vision_opencv/CHANGELOG.rst
index a423932..a6d8da7 100644
--- a/vision_opencv/CHANGELOG.rst
+++ b/vision_opencv/CHANGELOG.rst
@@ -2,6 +2,20 @@
 Changelog for package vision_opencv
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.12.2 (2016-09-24)
+-------------------
+
+1.12.1 (2016-07-11)
+-------------------
+
+1.12.0 (2016-03-18)
+-------------------
+* remove opencv_apps from vision_opencv
+* Contributors: Vincent Rabaud
+
+1.11.12 (2016-03-10)
+--------------------
+
 1.11.11 (2016-01-31)
 --------------------
 
diff --git a/vision_opencv/package.xml b/vision_opencv/package.xml
index 17c91db..bcb1ff2 100644
--- a/vision_opencv/package.xml
+++ b/vision_opencv/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>vision_opencv</name>
-  <version>1.11.11</version>
+  <version>1.12.2</version>
   <description>Packages for interfacing ROS with OpenCV, a library of programming functions for real time computer vision.</description>
   <author>Patrick Mihelich</author>
   <author>James Bowman</author>
@@ -13,10 +13,8 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   
-  <!-- Old stack contents -->
   <run_depend>cv_bridge</run_depend>
   <run_depend>image_geometry</run_depend>
-  <run_depend>opencv_apps</run_depend>
   <export>
     <metapackage/>
   </export>

-- 
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-vision-opencv.git



More information about the debian-science-commits mailing list