Merge branch 'master' of https://github.com/jsk-ros-pkg/jsk_recognition

.travis.yml

@@ -21,3 +21,7 @@ matrix:
 before_script:
   - export ROS_PARALLEL_JOBS="-j4 -l4"
 script: source .travis/travis.sh
+notifications:
+  email:
+    on_success: always
+    on_failure: always

jsk_pcl_ros/catkin.cmake

@@ -23,7 +23,8 @@ add_message_files(FILES PointsArray.msg ClusterPointIndices.msg Int32Stamped.msg
   ModelCoefficientsArray.msg
   SlicedPointCloud.msg
   BoundingBox.msg
-  BoundingBoxArray.msg)
+  BoundingBoxArray.msg
+  ColorHistogram.msg)
 add_service_files(FILES SwitchTopic.srv  TransformScreenpoint.srv CheckCircle.srv RobotPickupReleasePoint.srv  TowerPickUp.srv EuclideanSegment.srv TowerRobotMoveCommand.srv SetPointCloud2.srv
   CallSnapIt.srv CallPolygon.srv)
 

jsk_pcl_ros/msg/ColorHistogram.msg

@@ -0,0 +1,2 @@
+Header header
+float32[] histogram

jsk_perception/CMakeLists.txt

@@ -66,6 +66,7 @@ endmacro(jsk_perception_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec
 jsk_perception_nodelet(src/edge_detector.cpp "jsk_perception/EdgeDetector" "edge_detector")
 jsk_perception_nodelet(src/sparse_image_encoder.cpp "jsk_perception/SparseImageEncoder" "sparse_image_encoder")
 jsk_perception_nodelet(src/sparse_image_decoder.cpp "jsk_perception/SparseImageDecoder" "sparse_image_decoder")
+jsk_perception_nodelet(src/color_histogram.cpp "jsk_perception/ColorHistogram" "color_histogram")
 rosbuild_add_library (${PROJECT_NAME}
   ${jsk_perception_nodelet_sources}
   )

jsk_perception/catkin.cmake

@@ -1,12 +1,13 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(jsk_perception)
 
-find_package(catkin REQUIRED COMPONENTS message_generation imagesift std_msgs sensor_msgs geometry_msgs cv_bridge image_geometry image_transport driver_base dynamic_reconfigure pcl_ros eigen roscpp nodelet rostest)
+find_package(catkin REQUIRED COMPONENTS message_generation imagesift std_msgs sensor_msgs geometry_msgs cv_bridge image_geometry image_transport driver_base dynamic_reconfigure pcl_ros eigen roscpp nodelet rostest jsk_pcl_ros)
 find_package(OpenCV REQUIRED)
 find_package(Boost REQUIRED COMPONENTS filesystem system signals)
 
 # Dynamic reconfigure support
-generate_dynamic_reconfigure_options(cfg/camshiftdemo.cfg cfg/EdgeDetector.cfg cfg/HoughLines.cfg cfg/matchtemplate.cfg cfg/point_pose_extractor.cfg cfg/RectangleDetector.cfg)
+generate_dynamic_reconfigure_options(cfg/camshiftdemo.cfg cfg/EdgeDetector.cfg cfg/HoughLines.cfg cfg/matchtemplate.cfg cfg/point_pose_extractor.cfg cfg/RectangleDetector.cfg
+  cfg/ColorHistogram.cfg)
 
 add_message_files(FILES # ClusterPointIndices.msg
       PointsArray.msg RotatedRectStamped.msg LineArray.msg Rect.msg Line.msg RotatedRect.msg SparseImage.msg)
@@ -18,7 +19,7 @@ generate_messages(
 )
 
 catkin_package(
-  CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs message_runtime
+  CATKIN_DEPENDS std_msgs sensor_msgs geometry_msgs message_runtime jsk_pcl_ros
   DEPENDS
   INCLUDE_DIRS
   LIBRARIES
@@ -51,6 +52,7 @@ endmacro()
 jsk_perception_nodelet(src/edge_detector.cpp "jsk_perception/EdgeDetector" "edge_detector")
 jsk_perception_nodelet(src/sparse_image_encoder.cpp "jsk_perception/SparseImageEncoder" "sparse_image_encoder")
 jsk_perception_nodelet(src/sparse_image_decoder.cpp "jsk_perception/SparseImageDecoder" "sparse_image_decoder")
+jsk_perception_nodelet(src/color_histogram.cpp "jsk_perception/ColorHistogram" "color_histogram")
 
 # compiling jsk_perception library for nodelet
 add_library(${PROJECT_NAME} SHARED ${jsk_perception_nodelet_sources})

jsk_perception/cfg/ColorHistogram.cfg

@@ -0,0 +1,21 @@
+#! /usr/bin/env python
+
+PACKAGE='jsk_perception'
+try:
+    import imp
+    imp.find_module(PACKAGE)
+    from dynamic_reconfigure.parameter_generator_catkin import *
+except:
+    import roslib; roslib.load_manifest(PACKAGE)
+    from dynamic_reconfigure.parameter_generator import *
+
+gen = ParameterGenerator()
+
+gen.add("red_histogram_bin", int_t,    0, "the size of the bin for red", 1, 10, 256)
+gen.add("green_histogram_bin", int_t,    0, "the size of the bin for green", 1, 10, 256)
+gen.add("blue_histogram_bin", int_t,    0, "the size of the bin for blue", 1, 10, 256)
+gen.add("hue_histogram_bin", int_t,    0, "the size of the bin for hue", 1, 10, 180)
+gen.add("saturation_histogram_bin", int_t,    0, "the size of the bin for saturatoin", 1, 10, 256)
+gen.add("intensity_histogram_bin", int_t,    0, "the size of the bin for intensity", 1, 10, 256)
+
+exit(gen.generate(PACKAGE, "color_histogram", "ColorHistogram"))

jsk_perception/jsk_perception_nodelets.xml

@@ -1,4 +1,10 @@
 <library path="lib/libjsk_perception">
+  <class name="jsk_perception/ColorHistogram" type="ColorHistogram"
+         base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet to compute histogram
+    </description>
+  </class>
   <class name="jsk_perception/EdgeDetector" type="EdgeDetector"
          base_class_type="nodelet::Nodelet">
     <description>

jsk_perception/launch/color_histogram_sample.launch

@@ -0,0 +1,14 @@
+<launch>
+  <node pkg="usb_cam" type="usb_cam_node" name="usb_cam" />
+  <node pkg="jsk_perception" type="color_histogram" name="color_histogram">
+    <remap from="image" to="/usb_cam/image_raw" />
+  </node>
+  <node pkg="image_view2" type="image_view2" name="image_view2">
+    <remap from="image" to="/usb_cam/image_raw" />
+  </node>
+
+  <node pkg="jsk_rqt_plugins" type="rqt_histogram_plot" name="red_histogram"
+        args="/usb_cam/image_raw/red_histogram/histogram"/>
+  <node pkg="jsk_rqt_plugins" type="rqt_histogram_plot" name="hue_histogram"
+        args="/usb_cam/image_raw/hue_histogram/histogram"/>
+</launch>

jsk_perception/manifest.xml

@@ -26,6 +26,7 @@
   <!-- for pointcloud -->
   <depend package="pcl"/>
   <depend package="pcl_ros"/>
+  <depend package="jsk_pcl_ros"/>
 
   <!-- for build -->
   <depend package="rosbuild"/>

jsk_perception/package.xml

@@ -36,6 +36,7 @@
   <build_depend>angles</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>rostest</build_depend>
+  <build_depend>jsk_pcl_ros</build_depend>
 
   <run_depend>rospy</run_depend>
   <run_depend>roseus</run_depend>
@@ -57,6 +58,7 @@
   <run_depend>angles</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>rostest</run_depend>
+  <run_depend>jsk_pcl_ros</run_depend>
   <export>
     <nodelet plugin="${prefix}/jsk_perception_nodelets.xml"/>
   </export>

jsk_perception/src/color_histogram.cpp

@@ -0,0 +1,218 @@
+// -*- mode: C++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, JSK Lab
+ *  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/o2r other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <geometry_msgs/PolygonStamped.h>
+#include <jsk_pcl_ros/ColorHistogram.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <dynamic_reconfigure/server.h>
+#include <jsk_perception/ColorHistogramConfig.h>
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+namespace jsk_perception
+{
+  class ColorHistogram: public nodelet::Nodelet
+  {
+  public:
+    typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image,
+                                                             geometry_msgs::PolygonStamped > SyncPolicy;
+    typedef jsk_perception::ColorHistogramConfig Config;
+  protected:
+    boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
+    boost::shared_ptr<dynamic_reconfigure::Server<Config> > srv_;
+    image_transport::SubscriberFilter image_sub_;
+    message_filters::Subscriber<geometry_msgs::PolygonStamped> rectangle_sub_;
+    ros::NodeHandle nh_, pnh_;
+    boost::shared_ptr<image_transport::ImageTransport> it_;
+    ros::Publisher b_hist_pub_, r_hist_pub_, g_hist_pub_,
+      h_hist_pub_, s_hist_pub_, i_hist_pub_;
+    int b_hist_size_, r_hist_size_, g_hist_size_,
+      h_hist_size_, s_hist_size_, i_hist_size_;
+    boost::mutex mutex_;
+
+    void configCallback(Config &new_config, uint32_t level)
+    {
+      boost::mutex::scoped_lock(mutex_);
+      b_hist_size_ = new_config.blue_histogram_bin;
+      g_hist_size_ = new_config.green_histogram_bin;
+      r_hist_size_ = new_config.red_histogram_bin;
+      h_hist_size_ = new_config.hue_histogram_bin;
+      s_hist_size_ = new_config.saturation_histogram_bin;
+      i_hist_size_ = new_config.intensity_histogram_bin;
+    }
+    virtual void onInit()
+    {
+      nh_ = ros::NodeHandle(getNodeHandle(), "image");
+      pnh_ = ros::NodeHandle("~");
+      b_hist_size_ = r_hist_size_ = g_hist_size_ = h_hist_size_ = s_hist_size_ = i_hist_size_ = 512;
+      b_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("blue_histogram", 1);
+      g_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("green_histogram", 1);
+      r_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("red_histogram", 1);
+      h_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("hue_histogram", 1);
+      s_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("saturation_histogram", 1);
+      i_hist_pub_ = nh_.advertise<jsk_pcl_ros::ColorHistogram>("intensity_histogram", 1);
+      srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
+      dynamic_reconfigure::Server<Config>::CallbackType f =
+        boost::bind (&ColorHistogram::configCallback, this, _1, _2);
+      srv_->setCallback (f);
+
+      it_.reset(new image_transport::ImageTransport(nh_));
+      image_sub_.subscribe(*it_, "", 1);
+      rectangle_sub_.subscribe(nh_, "screenrectangle", 1);
+      sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
+      sync_->connectInput(image_sub_, rectangle_sub_);
+      sync_->registerCallback(boost::bind(&ColorHistogram::extract, this, _1, _2));
+    }
+
+    virtual void convertHistogramToMsg(const cv::Mat& hist,
+                                       int size,
+                                       jsk_pcl_ros::ColorHistogram& msg)
+    {
+      msg.histogram.clear();
+      for (int i = 0; i < size; i++) {
+        float val = hist.at<float>(0, i);
+        msg.histogram.push_back(val);
+      }
+    }
+
+    virtual void processBGR(const cv::Mat& bgr_image, const std_msgs::Header& header)
+    {
+
+      float range[] = { 0, 256 } ;
+      const float* histRange = { range };
+      cv::MatND b_hist, g_hist, r_hist;
+      bool uniform = true; bool accumulate = false;
+      std::vector<cv::Mat> bgr_planes;
+      split( bgr_image, bgr_planes );
+
+      cv::calcHist( &bgr_planes[0], 1, 0, cv::Mat(), b_hist, 1, &b_hist_size_, &histRange, uniform, accumulate );
+      cv::calcHist( &bgr_planes[1], 1, 0, cv::Mat(), g_hist, 1, &g_hist_size_, &histRange, uniform, accumulate );
+      cv::calcHist( &bgr_planes[2], 1, 0, cv::Mat(), r_hist, 1, &r_hist_size_, &histRange, uniform, accumulate );
+
+      jsk_pcl_ros::ColorHistogram b_histogram;
+      b_histogram.header = header;
+      convertHistogramToMsg(b_hist, b_hist_size_, b_histogram);
+      b_hist_pub_.publish(b_histogram);
+
+      jsk_pcl_ros::ColorHistogram g_histogram;
+      g_histogram.header = header;
+      convertHistogramToMsg(g_hist, g_hist_size_, g_histogram);
+      g_hist_pub_.publish(g_histogram);
+
+      jsk_pcl_ros::ColorHistogram r_histogram;
+      r_histogram.header = header;
+      convertHistogramToMsg(r_hist, r_hist_size_, r_histogram);
+      r_hist_pub_.publish(r_histogram);
+
+    }
+
+    virtual void processHSI(const cv::Mat& bgr_image, const std_msgs::Header& header)
+    {
+      cv::Mat hsi_image;
+      cv::cvtColor(bgr_image, hsi_image, CV_BGR2HSV);
+
+      float range[] = { 0, 256 } ;
+      const float* histRange = { range };
+      float h_range[] = { 0, 180 } ;
+      const float* h_histRange = { h_range };
+      cv::MatND h_hist, s_hist, i_hist;
+      bool uniform = true; bool accumulate = false;
+      std::vector<cv::Mat> hsi_planes;
+      split( hsi_image, hsi_planes );
+
+      cv::calcHist( &hsi_planes[0], 1, 0, cv::Mat(), h_hist, 1, &h_hist_size_, &h_histRange, uniform, accumulate );
+      cv::calcHist( &hsi_planes[1], 1, 0, cv::Mat(), s_hist, 1, &s_hist_size_, &histRange, uniform, accumulate );
+      cv::calcHist( &hsi_planes[2], 1, 0, cv::Mat(), i_hist, 1, &i_hist_size_, &histRange, uniform, accumulate );
+
+      jsk_pcl_ros::ColorHistogram h_histogram;
+      h_histogram.header = header;
+      convertHistogramToMsg(h_hist, h_hist_size_, h_histogram);
+      h_hist_pub_.publish(h_histogram);
+
+      jsk_pcl_ros::ColorHistogram s_histogram;
+      s_histogram.header = header;
+      convertHistogramToMsg(s_hist, s_hist_size_, s_histogram);
+      s_hist_pub_.publish(s_histogram);
+
+      jsk_pcl_ros::ColorHistogram i_histogram;
+      i_histogram.header = header;
+      convertHistogramToMsg(i_hist, i_hist_size_, i_histogram);
+      i_hist_pub_.publish(i_histogram);
+
+    }
+
+    virtual void extract(const sensor_msgs::Image::ConstPtr& image,
+                         const geometry_msgs::PolygonStamped::ConstPtr& rectangle)
+    {
+      boost::mutex::scoped_lock(mutex_);
+      try
+      {
+        cv_bridge::CvImagePtr cv_ptr;
+        cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
+
+        int larger_x_index = rectangle->polygon.points[0].x > rectangle->polygon.points[1].x? 0: 1;
+        int larger_y_index = rectangle->polygon.points[0].y > rectangle->polygon.points[1].y? 0: 1;
+        int smaller_x_index = rectangle->polygon.points[0].x < rectangle->polygon.points[1].x? 0: 1;
+        int smaller_y_index = rectangle->polygon.points[0].y < rectangle->polygon.points[1].y? 0: 1;
+        cv::Rect roi(rectangle->polygon.points[smaller_x_index].x,
+                     rectangle->polygon.points[smaller_y_index].y,
+                     rectangle->polygon.points[larger_x_index].x - rectangle->polygon.points[smaller_x_index].x,
+                     rectangle->polygon.points[larger_y_index].y - rectangle->polygon.points[smaller_y_index].y);
+        cv::Mat bgr_image = cv_ptr->image(roi);
+        processBGR(bgr_image, image->header);
+        processHSI(bgr_image, image->header);
+      }
+      catch (cv_bridge::Exception& e)
+      {
+        NODELET_ERROR("cv_bridge exception: %s", e.what());
+        return;
+      }
+    }
+  private:
+
+  };
+}
+
+#include <pluginlib/class_list_macros.h>
+typedef jsk_perception::ColorHistogram ColorHistogram;
+PLUGINLIB_DECLARE_CLASS (jsk_perception, ColorHistogram, ColorHistogram, nodelet::Nodelet);