diff --git a/image_proc/CMakeLists.txt b/image_proc/CMakeLists.txt index 2cc43a1e2..2e2a136c9 100644 --- a/image_proc/CMakeLists.txt +++ b/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;$\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;$\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) diff --git a/image_proc/include/image_proc/crop_non_zero.hpp b/image_proc/include/image_proc/crop_non_zero.hpp new file mode 100644 index 000000000..488e6e628 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + +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_ diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index f7d885397..68a6eb871 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -46,7 +46,11 @@ void debayer2x2toBGR( int R, int G1, int G2, int B) { typedef cv::Vec DstPixel; // 8- or 16-bit BGR + #if CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION > 2 + dst.create(src.rows / 2, src.cols / 2, cv::traits::Type::value); + #else dst.create(src.rows / 2, src.cols / 2, cv::DataType::type); + #endif int src_row_step = src.step1(); int dst_row_step = dst.step1(); diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp new file mode 100644 index 000000000..3a4c48861 --- /dev/null +++ b/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 +#include + +#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 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 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> 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>::iterator it = + std::max_element(cnt.begin(), cnt.end(), [](std::vector a, + std::vector 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)