From 00218358e83aa66832c4eb4a1dc951f54618235f Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 21 Apr 2017 15:51:22 +0900
Subject: [PATCH 1/6] color_filter_nodelet.cpp : publish color spaces

---
 src/nodelet/color_filter_nodelet.cpp | 89 ++++++++++++++++++++++++++++
 1 file changed, 89 insertions(+)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index 90af18e9..50a0d4f1 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -36,6 +36,7 @@
 #include "opencv_apps/nodelet.h"
 #include <image_transport/image_transport.h>
 #include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud.h>
 #include <cv_bridge/cv_bridge.h>
 
 #include <opencv2/highgui/highgui.hpp>
@@ -86,6 +87,10 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
 
   boost::mutex mutex_;
 
+  // publisch color space
+  sensor_msgs::PointCloud color_space_msg_;
+  ros::Publisher color_space_pub_;
+
   virtual void reconfigureCallback(Config& new_config, uint32_t level) = 0;
 
   virtual void filter(const cv::Mat& input_image, cv::Mat& output_image) = 0;
@@ -187,6 +192,9 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
 
     img_pub_ = advertiseImage(*pnh_, "image", 1);
 
+    // to advertise you can do it like this (as above):
+    color_space_pub_ = pnh_->advertise<sensor_msgs::PointCloud>("color_space", 1);
+
     onInitPostProcess();
   }
 };
@@ -229,6 +237,31 @@ class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFil
   void filter(const cv::Mat& input_image, cv::Mat& output_image)  // NOLINT(modernize-use-override)
   {
     cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
+
+    /// publish color spaces
+    color_space_msg_.header.frame_id = "/map";
+    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
+      color_space_msg_.points.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels.resize(3);
+      color_space_msg_.channels[0].name = "b";
+      color_space_msg_.channels[1].name = "g";
+      color_space_msg_.channels[2].name = "r";
+      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    }
+    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
+      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
+      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
+      color_space_msg_.points[i].x = r;
+      color_space_msg_.points[i].y = g;
+      color_space_msg_.points[i].z = b;
+      color_space_msg_.channels[0].values[i] = r;
+      color_space_msg_.channels[1].values[i] = g;
+      color_space_msg_.channels[2].values[i] = b;
+    }
+    color_space_pub_.publish(color_space_msg_);
   }
 
 protected:
@@ -300,6 +333,34 @@ class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFil
       cv::inRange(hls_image, lower_color_range_360, upper_color_range_360, output_image_360);
       output_image = output_image_0 | output_image_360;
     }
+
+    /// publish color spaces
+    color_space_msg_.header.frame_id = "/map";
+    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
+      color_space_msg_.points.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels.resize(3);
+      color_space_msg_.channels[0].name = "b";
+      color_space_msg_.channels[1].name = "g";
+      color_space_msg_.channels[2].name = "r";
+      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    }
+    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+      float h = hls_image.at<cv::Vec3b>(i)[0]*2;
+      float l = hls_image.at<cv::Vec3b>(i)[1]/255.0;
+      float s = hls_image.at<cv::Vec3b>(i)[2]/255.0;
+      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
+      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
+      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
+      color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
+      color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
+      color_space_msg_.points[i].z = l;
+      color_space_msg_.channels[0].values[i] = r;
+      color_space_msg_.channels[1].values[i] = g;
+      color_space_msg_.channels[2].values[i] = b;
+    }
+    color_space_pub_.publish(color_space_msg_);
   }
 
 public:
@@ -368,6 +429,34 @@ class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFil
       cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
       output_image = output_image_0 | output_image_360;
     }
+
+    /// publish color spaces
+    color_space_msg_.header.frame_id = "/map";
+    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
+      color_space_msg_.points.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels.resize(3);
+      color_space_msg_.channels[0].name = "b";
+      color_space_msg_.channels[1].name = "g";
+      color_space_msg_.channels[2].name = "r";
+      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
+      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    }
+    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+      float h = hsv_image.at<cv::Vec3b>(i)[0]*2;
+      float s = hsv_image.at<cv::Vec3b>(i)[1]/255.0;
+      float v = hsv_image.at<cv::Vec3b>(i)[2]/255.0;
+      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
+      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
+      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
+      color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
+      color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
+      color_space_msg_.points[i].z = v;
+      color_space_msg_.channels[0].values[i] = r;
+      color_space_msg_.channels[1].values[i] = g;
+      color_space_msg_.channels[2].values[i] = b;
+    }
+    color_space_pub_.publish(color_space_msg_);
   }
 
 public:

From 7faae6dced24bd39ab68c0b53577c270a207d860 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 21 Apr 2017 21:28:29 +0900
Subject: [PATCH 2/6] color_filter_nodelet.cpp : use PointCloud2 instead of
 PointCloud

---
 src/nodelet/color_filter_nodelet.cpp | 133 +++++++++++++++------------
 1 file changed, 72 insertions(+), 61 deletions(-)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index 50a0d4f1..352e8046 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -36,7 +36,7 @@
 #include "opencv_apps/nodelet.h"
 #include <image_transport/image_transport.h>
 #include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
 #include <cv_bridge/cv_bridge.h>
 
 #include <opencv2/highgui/highgui.hpp>
@@ -88,7 +88,7 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
   boost::mutex mutex_;
 
   // publisch color space
-  sensor_msgs::PointCloud color_space_msg_;
+  sensor_msgs::PointCloud2 color_space_msg_;
   ros::Publisher color_space_pub_;
 
   virtual void reconfigureCallback(Config& new_config, uint32_t level) = 0;
@@ -193,7 +193,25 @@ class ColorFilterNodelet : public opencv_apps::Nodelet
     img_pub_ = advertiseImage(*pnh_, "image", 1);
 
     // to advertise you can do it like this (as above):
-    color_space_pub_ = pnh_->advertise<sensor_msgs::PointCloud>("color_space", 1);
+    color_space_pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("color_space", 1);
+    color_space_msg_.header.frame_id = "/map";
+    color_space_msg_.fields.resize(4);
+    color_space_msg_.fields[0].name = "x";
+    color_space_msg_.fields[0].offset = 0;
+    color_space_msg_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
+    color_space_msg_.fields[0].count = 1;
+    color_space_msg_.fields[1].name = "y";
+    color_space_msg_.fields[1].offset = 4;
+    color_space_msg_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
+    color_space_msg_.fields[1].count = 1;
+    color_space_msg_.fields[2].name = "z";
+    color_space_msg_.fields[2].offset = 8;
+    color_space_msg_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
+    color_space_msg_.fields[2].count = 1;
+    color_space_msg_.fields[3].name = "rgb";
+    color_space_msg_.fields[3].offset = 12;
+    color_space_msg_.fields[3].datatype = sensor_msgs::PointField::UINT32;
+    color_space_msg_.fields[3].count = 1;
 
     onInitPostProcess();
   }
@@ -239,27 +257,25 @@ class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFil
     cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
 
     /// publish color spaces
-    color_space_msg_.header.frame_id = "/map";
-    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
-      color_space_msg_.points.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels.resize(3);
-      color_space_msg_.channels[0].name = "b";
-      color_space_msg_.channels[1].name = "g";
-      color_space_msg_.channels[2].name = "r";
-      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
+      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+      color_space_msg_.width = input_image.cols;
+      color_space_msg_.height = input_image.rows;
+      color_space_msg_.point_step = 16;
+      color_space_msg_.row_step = color_space_msg_.width;
     }
     for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
-      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
-      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
-      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
-      color_space_msg_.points[i].x = r;
-      color_space_msg_.points[i].y = g;
-      color_space_msg_.points[i].z = b;
-      color_space_msg_.channels[0].values[i] = r;
-      color_space_msg_.channels[1].values[i] = g;
-      color_space_msg_.channels[2].values[i] = b;
+      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
+      float x = r/255.0;
+      float y = g/255.0;
+      float z = b/255.0;
+      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      unsigned char rgb[4] = {r, g, b, 0};
+      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }
@@ -335,30 +351,28 @@ class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFil
     }
 
     /// publish color spaces
-    color_space_msg_.header.frame_id = "/map";
-    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
-      color_space_msg_.points.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels.resize(3);
-      color_space_msg_.channels[0].name = "b";
-      color_space_msg_.channels[1].name = "g";
-      color_space_msg_.channels[2].name = "r";
-      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
+      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+      color_space_msg_.width = input_image.cols;
+      color_space_msg_.height = input_image.rows;
+      color_space_msg_.point_step = 16;
+      color_space_msg_.row_step = color_space_msg_.width;
     }
     for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
       float h = hls_image.at<cv::Vec3b>(i)[0]*2;
       float l = hls_image.at<cv::Vec3b>(i)[1]/255.0;
       float s = hls_image.at<cv::Vec3b>(i)[2]/255.0;
-      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
-      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
-      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
-      color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
-      color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
-      color_space_msg_.points[i].z = l;
-      color_space_msg_.channels[0].values[i] = r;
-      color_space_msg_.channels[1].values[i] = g;
-      color_space_msg_.channels[2].values[i] = b;
+      float x = s*cos(h*M_PI/180.0);
+      float y = s*sin(h*M_PI/180.0);
+      float z = l;
+      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      unsigned char rgb[4] = {r, g, b, 0};
+      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }
@@ -429,32 +443,29 @@ class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFil
       cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
       output_image = output_image_0 | output_image_360;
     }
-
     /// publish color spaces
-    color_space_msg_.header.frame_id = "/map";
-    if ( color_space_msg_.points.size() != input_image.rows*input_image.cols) {
-      color_space_msg_.points.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels.resize(3);
-      color_space_msg_.channels[0].name = "b";
-      color_space_msg_.channels[1].name = "g";
-      color_space_msg_.channels[2].name = "r";
-      color_space_msg_.channels[0].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[1].values.resize(input_image.rows*input_image.cols);
-      color_space_msg_.channels[2].values.resize(input_image.rows*input_image.cols);
+    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
+      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+      color_space_msg_.width = input_image.cols;
+      color_space_msg_.height = input_image.rows;
+      color_space_msg_.point_step = 16;
+      color_space_msg_.row_step = color_space_msg_.width;
     }
     for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
       float h = hsv_image.at<cv::Vec3b>(i)[0]*2;
       float s = hsv_image.at<cv::Vec3b>(i)[1]/255.0;
       float v = hsv_image.at<cv::Vec3b>(i)[2]/255.0;
-      float r = input_image.at<cv::Vec3b>(i)[0]/255.0;
-      float g = input_image.at<cv::Vec3b>(i)[1]/255.0;
-      float b = input_image.at<cv::Vec3b>(i)[2]/255.0;
-      color_space_msg_.points[i].x = s*cos(h*M_PI/180.0);
-      color_space_msg_.points[i].y = s*sin(h*M_PI/180.0);
-      color_space_msg_.points[i].z = v;
-      color_space_msg_.channels[0].values[i] = r;
-      color_space_msg_.channels[1].values[i] = g;
-      color_space_msg_.channels[2].values[i] = b;
+      float x = s*cos(h*M_PI/180.0);
+      float y = s*sin(h*M_PI/180.0);
+      float z = v;
+      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
+      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      unsigned char rgb[4] = {r, g, b, 0};
+      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }

From 5a0ccb97838605946e2f506b6042a0c5d1940bf8 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Sun, 23 Apr 2017 18:22:20 +0900
Subject: [PATCH 3/6] output with gray if the point is not within the threshold

---
 src/nodelet/color_filter_nodelet.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index 352e8046..3f0ea124 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -60,6 +60,19 @@ class RGBColorFilter;
 class HLSColorFilter;
 class HSVColorFilter;
 
+// http://stackoverflow.com/questions/12703305/how-to-compare-scalars-in-opencv
+// Custom operator to compare cv::Scalar class...
+bool operator <(const cv::Scalar &a, const cv::Scalar &b)
+{
+  bool Result = true;
+  // Do whatever you think a Scalar comparison must be.
+  std::cerr << a.depth <<  " " << a.channels << std::endl;
+  for ( size_t i = 0 ; i < 4; i++ ) {
+    if ( a[i] >= b[i] ) Result = false;
+  }
+  return Result;
+}
+
 template <typename Config>
 class ColorFilterNodelet : public opencv_apps::Nodelet
 {
@@ -274,6 +287,10 @@ class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFil
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
@@ -371,6 +388,10 @@ class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFil
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }
@@ -464,6 +485,10 @@ class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFil
       memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
       memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
+      if ( output_image.at<unsigned char>(i) == 0 ) {
+        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+        r = g = b = gray;
+      }
       unsigned char rgb[4] = {r, g, b, 0};
       memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
     }

From e1fa27e4b1e8e8434cb023d1f44c96906a814d1a Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 29 Nov 2019 17:38:07 +0900
Subject: [PATCH 4/6] fix to fit clang-format / clang-tidy

---
 src/nodelet/color_filter_nodelet.cpp | 109 +++++++++++++++------------
 1 file changed, 60 insertions(+), 49 deletions(-)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index 3f0ea124..1a4c166c 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -62,15 +62,17 @@ class HSVColorFilter;
 
 // http://stackoverflow.com/questions/12703305/how-to-compare-scalars-in-opencv
 // Custom operator to compare cv::Scalar class...
-bool operator <(const cv::Scalar &a, const cv::Scalar &b)
+bool operator<(const cv::Scalar& a, const cv::Scalar& b)
 {
-  bool Result = true;
+  bool result = true;
   // Do whatever you think a Scalar comparison must be.
-  std::cerr << a.depth <<  " " << a.channels << std::endl;
-  for ( size_t i = 0 ; i < 4; i++ ) {
-    if ( a[i] >= b[i] ) Result = false;
+  std::cerr << a.depth << " " << a.channels << std::endl;
+  for (size_t i = 0; i < 4; i++)
+  {
+    if (a[i] >= b[i])
+      result = false;
   }
-  return Result;
+  return result;
 }
 
 template <typename Config>
@@ -270,29 +272,32 @@ class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFil
     cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
 
     /// publish color spaces
-    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
-      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
+    {
+      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
       color_space_msg_.width = input_image.cols;
       color_space_msg_.height = input_image.rows;
       color_space_msg_.point_step = 16;
       color_space_msg_.row_step = color_space_msg_.width;
     }
-    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+    {
       unsigned char r = input_image.at<cv::Vec3b>(i)[0];
       unsigned char g = input_image.at<cv::Vec3b>(i)[1];
       unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float x = r/255.0;
-      float y = g/255.0;
-      float z = b/255.0;
-      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
-      if ( output_image.at<unsigned char>(i) == 0 ) {
-        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+      float x = r / 255.0;
+      float y = g / 255.0;
+      float z = b / 255.0;
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+      if (output_image.at<unsigned char>(i) == 0)
+      {
+        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
         r = g = b = gray;
       }
-      unsigned char rgb[4] = {r, g, b, 0};
-      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
+      unsigned char rgb[4] = { r, g, b, 0 };
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }
@@ -368,32 +373,35 @@ class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFil
     }
 
     /// publish color spaces
-    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
-      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
+    {
+      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
       color_space_msg_.width = input_image.cols;
       color_space_msg_.height = input_image.rows;
       color_space_msg_.point_step = 16;
       color_space_msg_.row_step = color_space_msg_.width;
     }
-    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+    {
       unsigned char r = input_image.at<cv::Vec3b>(i)[0];
       unsigned char g = input_image.at<cv::Vec3b>(i)[1];
       unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float h = hls_image.at<cv::Vec3b>(i)[0]*2;
-      float l = hls_image.at<cv::Vec3b>(i)[1]/255.0;
-      float s = hls_image.at<cv::Vec3b>(i)[2]/255.0;
-      float x = s*cos(h*M_PI/180.0);
-      float y = s*sin(h*M_PI/180.0);
+      float h = hls_image.at<cv::Vec3b>(i)[0] * 2;
+      float l = hls_image.at<cv::Vec3b>(i)[1] / 255.0;
+      float s = hls_image.at<cv::Vec3b>(i)[2] / 255.0;
+      float x = s * cos(h * M_PI / 180.0);
+      float y = s * sin(h * M_PI / 180.0);
       float z = l;
-      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
-      if ( output_image.at<unsigned char>(i) == 0 ) {
-        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+      if (output_image.at<unsigned char>(i) == 0)
+      {
+        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
         r = g = b = gray;
       }
-      unsigned char rgb[4] = {r, g, b, 0};
-      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
+      unsigned char rgb[4] = { r, g, b, 0 };
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }
@@ -465,32 +473,35 @@ class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFil
       output_image = output_image_0 | output_image_360;
     }
     /// publish color spaces
-    if ( color_space_msg_.data.size() != 16*input_image.rows*input_image.cols) {
-      color_space_msg_.data.resize(16*input_image.rows*input_image.cols);
+    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
+    {
+      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
       color_space_msg_.width = input_image.cols;
       color_space_msg_.height = input_image.rows;
       color_space_msg_.point_step = 16;
       color_space_msg_.row_step = color_space_msg_.width;
     }
-    for ( size_t i = 0; i < input_image.rows *  input_image.cols; i++ ) {
+    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+    {
       unsigned char r = input_image.at<cv::Vec3b>(i)[0];
       unsigned char g = input_image.at<cv::Vec3b>(i)[1];
       unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float h = hsv_image.at<cv::Vec3b>(i)[0]*2;
-      float s = hsv_image.at<cv::Vec3b>(i)[1]/255.0;
-      float v = hsv_image.at<cv::Vec3b>(i)[2]/255.0;
-      float x = s*cos(h*M_PI/180.0);
-      float y = s*sin(h*M_PI/180.0);
+      float h = hsv_image.at<cv::Vec3b>(i)[0] * 2;
+      float s = hsv_image.at<cv::Vec3b>(i)[1] / 255.0;
+      float v = hsv_image.at<cv::Vec3b>(i)[2] / 255.0;
+      float x = s * cos(h * M_PI / 180.0);
+      float y = s * sin(h * M_PI / 180.0);
       float z = v;
-      memcpy((void *)(&(color_space_msg_.data[i*16+0])), (const void *)&x, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+4])), (const void *)&y, sizeof(float));
-      memcpy((void *)(&(color_space_msg_.data[i*16+8])), (const void *)&z, sizeof(float));
-      if ( output_image.at<unsigned char>(i) == 0 ) {
-        unsigned char gray = 16 + (r/3 + g/3 + b/3)*(255-16)/255;
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+      if (output_image.at<unsigned char>(i) == 0)
+      {
+        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
         r = g = b = gray;
       }
-      unsigned char rgb[4] = {r, g, b, 0};
-      memcpy((void *)(&(color_space_msg_.data[i*16+12])), (const void *)rgb, 4*sizeof(unsigned char));
+      unsigned char rgb[4] = { r, g, b, 0 };
+      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
     }
     color_space_pub_.publish(color_space_msg_);
   }

From 6019f54752b3cc9a4c04c8511eca6d92ef6b19e2 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 29 Nov 2019 17:39:11 +0900
Subject: [PATCH 5/6] remove debug code

---
 src/nodelet/color_filter_nodelet.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index 1a4c166c..fa68766c 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -66,7 +66,6 @@ bool operator<(const cv::Scalar& a, const cv::Scalar& b)
 {
   bool result = true;
   // Do whatever you think a Scalar comparison must be.
-  std::cerr << a.depth << " " << a.channels << std::endl;
   for (size_t i = 0; i < 4; i++)
   {
     if (a[i] >= b[i])

From b2fe0aa3dfac279dd3045d2febd7fe89f073f915 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Sun, 1 Dec 2019 18:16:02 +0900
Subject: [PATCH 6/6] publish color_space_pub_ when when subscribed

---
 src/nodelet/color_filter_nodelet.cpp | 171 ++++++++++++++-------------
 1 file changed, 90 insertions(+), 81 deletions(-)

diff --git a/src/nodelet/color_filter_nodelet.cpp b/src/nodelet/color_filter_nodelet.cpp
index fa68766c..2986be99 100644
--- a/src/nodelet/color_filter_nodelet.cpp
+++ b/src/nodelet/color_filter_nodelet.cpp
@@ -270,35 +270,38 @@ class RGBColorFilterNodelet : public ColorFilterNodelet<opencv_apps::RGBColorFil
   {
     cv::inRange(input_image, lower_color_range_, upper_color_range_, output_image);
 
-    /// publish color spaces
-    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
+    if (color_space_pub_.getNumSubscribers() > 0)
     {
-      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
-      color_space_msg_.width = input_image.cols;
-      color_space_msg_.height = input_image.rows;
-      color_space_msg_.point_step = 16;
-      color_space_msg_.row_step = color_space_msg_.width;
-    }
-    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
-    {
-      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
-      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
-      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float x = r / 255.0;
-      float y = g / 255.0;
-      float z = b / 255.0;
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
-      if (output_image.at<unsigned char>(i) == 0)
+      /// publish color spaces
+      if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
       {
-        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
-        r = g = b = gray;
+        color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
+        color_space_msg_.width = input_image.cols;
+        color_space_msg_.height = input_image.rows;
+        color_space_msg_.point_step = 16;
+        color_space_msg_.row_step = color_space_msg_.width;
       }
-      unsigned char rgb[4] = { r, g, b, 0 };
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
+      for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+      {
+        unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+        unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+        unsigned char b = input_image.at<cv::Vec3b>(i)[2];
+        float x = r / 255.0;
+        float y = g / 255.0;
+        float z = b / 255.0;
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+        if (output_image.at<unsigned char>(i) == 0)
+        {
+          unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
+          r = g = b = gray;
+        }
+        unsigned char rgb[4] = { r, g, b, 0 };
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
+      }
+      color_space_pub_.publish(color_space_msg_);
     }
-    color_space_pub_.publish(color_space_msg_);
   }
 
 protected:
@@ -371,38 +374,41 @@ class HLSColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HLSColorFil
       output_image = output_image_0 | output_image_360;
     }
 
-    /// publish color spaces
-    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
-    {
-      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
-      color_space_msg_.width = input_image.cols;
-      color_space_msg_.height = input_image.rows;
-      color_space_msg_.point_step = 16;
-      color_space_msg_.row_step = color_space_msg_.width;
-    }
-    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+    if (color_space_pub_.getNumSubscribers() > 0)
     {
-      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
-      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
-      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float h = hls_image.at<cv::Vec3b>(i)[0] * 2;
-      float l = hls_image.at<cv::Vec3b>(i)[1] / 255.0;
-      float s = hls_image.at<cv::Vec3b>(i)[2] / 255.0;
-      float x = s * cos(h * M_PI / 180.0);
-      float y = s * sin(h * M_PI / 180.0);
-      float z = l;
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
-      if (output_image.at<unsigned char>(i) == 0)
+      /// publish color spaces
+      if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
+      {
+        color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
+        color_space_msg_.width = input_image.cols;
+        color_space_msg_.height = input_image.rows;
+        color_space_msg_.point_step = 16;
+        color_space_msg_.row_step = color_space_msg_.width;
+      }
+      for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
       {
-        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
-        r = g = b = gray;
+        unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+        unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+        unsigned char b = input_image.at<cv::Vec3b>(i)[2];
+        float h = hls_image.at<cv::Vec3b>(i)[0] * 2;
+        float l = hls_image.at<cv::Vec3b>(i)[1] / 255.0;
+        float s = hls_image.at<cv::Vec3b>(i)[2] / 255.0;
+        float x = s * cos(h * M_PI / 180.0);
+        float y = s * sin(h * M_PI / 180.0);
+        float z = l;
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+        if (output_image.at<unsigned char>(i) == 0)
+        {
+          unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
+          r = g = b = gray;
+        }
+        unsigned char rgb[4] = { r, g, b, 0 };
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
       }
-      unsigned char rgb[4] = { r, g, b, 0 };
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
+      color_space_pub_.publish(color_space_msg_);
     }
-    color_space_pub_.publish(color_space_msg_);
   }
 
 public:
@@ -471,38 +477,41 @@ class HSVColorFilterNodelet : public ColorFilterNodelet<opencv_apps::HSVColorFil
       cv::inRange(hsv_image, lower_color_range_360, upper_color_range_360, output_image_360);
       output_image = output_image_0 | output_image_360;
     }
-    /// publish color spaces
-    if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
-    {
-      color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
-      color_space_msg_.width = input_image.cols;
-      color_space_msg_.height = input_image.rows;
-      color_space_msg_.point_step = 16;
-      color_space_msg_.row_step = color_space_msg_.width;
-    }
-    for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+    if (color_space_pub_.getNumSubscribers() > 0)
     {
-      unsigned char r = input_image.at<cv::Vec3b>(i)[0];
-      unsigned char g = input_image.at<cv::Vec3b>(i)[1];
-      unsigned char b = input_image.at<cv::Vec3b>(i)[2];
-      float h = hsv_image.at<cv::Vec3b>(i)[0] * 2;
-      float s = hsv_image.at<cv::Vec3b>(i)[1] / 255.0;
-      float v = hsv_image.at<cv::Vec3b>(i)[2] / 255.0;
-      float x = s * cos(h * M_PI / 180.0);
-      float y = s * sin(h * M_PI / 180.0);
-      float z = v;
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
-      if (output_image.at<unsigned char>(i) == 0)
+      /// publish color spaces
+      if (color_space_msg_.data.size() != 16 * input_image.rows * input_image.cols)
       {
-        unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
-        r = g = b = gray;
+        color_space_msg_.data.resize(16 * input_image.rows * input_image.cols);
+        color_space_msg_.width = input_image.cols;
+        color_space_msg_.height = input_image.rows;
+        color_space_msg_.point_step = 16;
+        color_space_msg_.row_step = color_space_msg_.width;
+      }
+      for (size_t i = 0; i < input_image.rows * input_image.cols; i++)
+      {
+        unsigned char r = input_image.at<cv::Vec3b>(i)[0];
+        unsigned char g = input_image.at<cv::Vec3b>(i)[1];
+        unsigned char b = input_image.at<cv::Vec3b>(i)[2];
+        float h = hsv_image.at<cv::Vec3b>(i)[0] * 2;
+        float s = hsv_image.at<cv::Vec3b>(i)[1] / 255.0;
+        float v = hsv_image.at<cv::Vec3b>(i)[2] / 255.0;
+        float x = s * cos(h * M_PI / 180.0);
+        float y = s * sin(h * M_PI / 180.0);
+        float z = v;
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 0])), (const void*)&x, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 4])), (const void*)&y, sizeof(float));
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 8])), (const void*)&z, sizeof(float));
+        if (output_image.at<unsigned char>(i) == 0)
+        {
+          unsigned char gray = 16 + (r / 3 + g / 3 + b / 3) * (255 - 16) / 255;
+          r = g = b = gray;
+        }
+        unsigned char rgb[4] = { r, g, b, 0 };
+        memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
       }
-      unsigned char rgb[4] = { r, g, b, 0 };
-      memcpy((void*)(&(color_space_msg_.data[i * 16 + 12])), (const void*)rgb, 4 * sizeof(unsigned char));
+      color_space_pub_.publish(color_space_msg_);
     }
-    color_space_pub_.publish(color_space_msg_);
   }
 
 public: