Skip to content

Commit

Permalink
Merge pull request #459 from klintan/image-proc-resize-crop
Browse files Browse the repository at this point in the history
ROS2: Image proc non zero crop
  • Loading branch information
SteveMacenski committed Nov 26, 2019
2 parents 9a143c7 + 42ed31e commit bb4c2db
Show file tree
Hide file tree
Showing 4 changed files with 248 additions and 8 deletions.
39 changes: 31 additions & 8 deletions image_proc/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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)
Expand Down
71 changes: 71 additions & 0 deletions image_proc/include/image_proc/crop_non_zero.hpp
Original file line number Diff line number Diff line change
@@ -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_
4 changes: 4 additions & 0 deletions image_proc/src/crop_decimate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
#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();
Expand Down
142 changes: 142 additions & 0 deletions image_proc/src/crop_non_zero.cpp
Original file line number Diff line number Diff line change
@@ -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)

0 comments on commit bb4c2db

Please sign in to comment.