ROS2: Image proc non zero crop

image_proc/CMakeLists.txt

@@ -89,6 +89,19 @@ ament_target_dependencies(crop_decimate
 
 rclcpp_components_register_nodes(crop_decimate "image_proc::CropDecimateNode")
 set(node_plugins "${node_plugins}image_proc::CropDecimateNode;$<TARGET_FILE:crop_decimate>\n")
+add_library(crop_non_zero SHARED
+  src/crop_non_zero.cpp
+)
+
+target_compile_definitions(crop_non_zero
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+
+ament_target_dependencies(crop_non_zero
+  ${dependencies}
+)
+
+rclcpp_components_register_nodes(crop_non_zero "image_proc::CropNonZeroNode")
+set(node_plugins "${node_plugins}image_proc::CropNonZeroNode;$<TARGET_FILE:crop_non_zero>\n")
 
 add_executable(image_proc
   src/image_proc.cpp
@@ -112,16 +125,17 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+
 install(
-  TARGETS resize
-  EXPORT export_resize
+  TARGETS crop_decimate
+  EXPORT export_crop_decimate
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 install(
-  TARGETS crop_decimate
-  EXPORT export_crop_decimate
+  TARGETS crop_non_zero
+  EXPORT export_crop_non_zero
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -140,19 +154,28 @@ install(
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
+install(
+  TARGETS resize
+  EXPORT export_resize
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
 
 ament_export_interfaces(
-  export_resize HAS_LIBRARY_TARGET
+  export_crop_decimate HAS_LIBRARY_TARGET
+  export_crop_non_zero HAS_LIBRARY_TARGET
   export_debayer HAS_LIBRARY_TARGET
   export_rectify HAS_LIBRARY_TARGET
-  export_crop_decimate HAS_LIBRARY_TARGET
+  export_resize HAS_LIBRARY_TARGET
 )
 
 ament_export_libraries(
-  resize
+  crop_decimate
+  crop_non_zero
   debayer
   rectify
-  crop_decimate
+  resize
 )
 
 if(BUILD_TESTING)

image_proc/include/image_proc/crop_non_zero.hpp

@@ -0,0 +1,71 @@
+// Copyright 2008, 2019 Willow Garage, Inc., Steve Macenski, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 {copyright_holder} 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.
+
+#ifndef IMAGE_PROC__CROP_NON_ZERO_HPP_
+#define IMAGE_PROC__CROP_NON_ZERO_HPP_
+
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <thread>
+#include <memory>
+#include <vector>
+#include <string>
+
+namespace image_proc
+{
+
+class CropNonZeroNode : public rclcpp::Node
+{
+public:
+  explicit CropNonZeroNode(const rclcpp::NodeOptions &);
+
+private:
+  std::string camera_namespace_;
+  std::string image_pub_topic_;
+  std::string image_sub_topic_;
+
+  // Subscriptions
+  image_transport::Subscriber sub_raw_;
+
+  // Publications
+  std::mutex connect_mutex_;
+
+  image_transport::Publisher pub_;
+
+  void connectCb();
+
+  void imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg);
+};
+}  // namespace image_proc
+#endif  // IMAGE_PROC__CROP_NON_ZERO_HPP_

image_proc/src/crop_decimate.cpp

@@ -46,7 +46,11 @@ void debayer2x2toBGR(
   int R, int G1, int G2, int B)
 {
   typedef cv::Vec<T, 3> DstPixel;  // 8- or 16-bit BGR
-  dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
+  #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION > 2
+      dst.create(src.rows / 2, src.cols / 2, cv::traits::Type<DstPixel>::value);
+  #else
+      dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
+  #endif
 
   int src_row_step = src.step1();
   int dst_row_step = dst.step1();

image_proc/src/crop_non_zero.cpp

@@ -0,0 +1,142 @@
+// Copyright 2008, 2019 Willow Garage, Inc., Steve Macenski, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// 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 {copyright_holder} 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 <algorithm>
+#include <vector>
+
+#include "image_proc/crop_non_zero.hpp"
+
+
+namespace image_proc
+{
+
+namespace enc = sensor_msgs::image_encodings;
+
+CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
+: Node("CropNonZeroNode", options)
+{
+  auto parameter_change_cb =
+    [this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult {
+      auto result = rcl_interfaces::msg::SetParametersResult();
+      result.successful = true;
+      for (auto parameter : parameters) {
+        if (parameter.get_name() == "camera_namespace") {
+          camera_namespace_ = parameter.as_string();
+          RCLCPP_INFO(get_logger(), "camera_namespace: %s ", camera_namespace_.c_str());
+          break;
+        }
+      }
+      image_pub_topic_ = camera_namespace_ + "/image";
+
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      connectCb();
+      // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
+      RCLCPP_INFO(this->get_logger(), "publish: %s", image_pub_topic_.c_str());
+      pub_ = image_transport::create_publisher(this, image_pub_topic_);
+      return result;
+    };
+  this->declare_parameter("camera_namespace");
+
+  rclcpp::Parameter parameter;
+  if (rclcpp::PARAMETER_NOT_SET != this->get_parameter("camera_namespace", parameter)) {
+    parameter_change_cb(this->get_parameters({"camera_namespace"}));
+  }
+
+  this->set_on_parameters_set_callback(parameter_change_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void CropNonZeroNode::connectCb()
+{
+  image_sub_topic_ = camera_namespace_ + "/image_raw";
+
+  RCLCPP_INFO(this->get_logger(), "subscribe: %s", image_sub_topic_.c_str());
+  sub_raw_ = image_transport::create_subscription(this, image_sub_topic_,
+      std::bind(&CropNonZeroNode::imageCb, this, std::placeholders::_1), "raw");
+}
+
+void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
+{
+  cv_bridge::CvImagePtr cv_ptr;
+  try {
+    cv_ptr = cv_bridge::toCvCopy(raw_msg);
+  } catch (cv_bridge::Exception & e) {
+    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
+    return;
+  }
+
+  // Check the number of channels
+  if (sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1) {
+    RCLCPP_ERROR(this->get_logger(), "Only grayscale image is acceptable, got [%s]",
+      raw_msg->encoding.c_str());
+    return;
+  }
+  std::vector<std::vector<cv::Point>> cnt;
+  cv::Mat1b m(raw_msg->width, raw_msg->height);
+
+  if (raw_msg->encoding == enc::TYPE_8UC1) {
+    m = cv_ptr->image;
+  } else {
+    double minVal, maxVal;
+    cv::minMaxIdx(cv_ptr->image, &minVal, &maxVal, 0, 0, cv_ptr->image != 0.);
+    double ra = maxVal - minVal;
+
+    cv_ptr->image.convertTo(m, CV_8U, 255. / ra, -minVal * 255. / ra);
+  }
+
+  cv::findContours(m, cnt, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+
+  // search the largest area
+  std::vector<std::vector<cv::Point>>::iterator it =
+    std::max_element(cnt.begin(), cnt.end(), [](std::vector<cv::Point> a,
+      std::vector<cv::Point> b) {
+        return a.size() < b.size();
+      });
+
+  cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]);
+
+  cv_bridge::CvImage out_msg;
+  out_msg.header = raw_msg->header;
+  out_msg.encoding = raw_msg->encoding;
+  out_msg.image = cv_ptr->image(r);
+
+  pub_.publish(out_msg.toImageMsg());
+}
+
+}  // namespace image_proc
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(image_proc::CropNonZeroNode)