Merge pull request #2726 from iory/invert-mask-image

[jsk_perception] Add invert_mask_image nodelet

doc/jsk_perception/nodes/invert_mask_image.md

@@ -0,0 +1,29 @@
+# InvertMaskImage
+
+![](images/invert_mask_image.jpg)
+
+Inverts the input mask image.
+
+## Subscribing Topic
+
+* `~input` (`sensor_msgs/Image`)
+
+  Input mask image.
+
+## Publishing Topic
+
+* `~output` (`sensor_msgs/Image`)
+
+  Inverted mask image.
+
+## Parameters
+
+* `~queue_size` (Int, default: `1`)
+
+  How many messages you allow about the subscriber to keep in the queue.
+
+## Sample
+
+```bash
+roslaunch jsk_perception sample_invert_mask_image.launch
+```

jsk_perception/CMakeLists.txt

@@ -260,6 +260,7 @@ jsk_add_nodelet(src/overlay_image_color_on_mono.cpp "jsk_perception/OverlayImage
 jsk_add_nodelet(src/grid_label.cpp "jsk_perception/GridLabel" "grid_label")
 jsk_add_nodelet(src/color_histogram_label_match.cpp "jsk_perception/ColorHistogramLabelMatch" "color_histogram_label_match")
 jsk_add_nodelet(src/apply_mask_image.cpp "jsk_perception/ApplyMaskImage" "apply_mask_image")
+jsk_add_nodelet(src/invert_mask_image.cpp "jsk_perception/InvertMaskImage" "invert_mask_image")
 jsk_add_nodelet(src/unapply_mask_image.cpp "jsk_perception/UnapplyMaskImage" "unapply_mask_image")
 jsk_add_nodelet(src/single_channel_histogram.cpp "jsk_perception/SingleChannelHistogram" "single_channel_histogram")
 jsk_add_nodelet(src/blob_detector.cpp "jsk_perception/BlobDetector" "blob_detector")

jsk_perception/include/jsk_perception/invert_mask_image.h

@@ -0,0 +1,66 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, 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/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the JSK Lab 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 JSK_PERCEPTION_INVERT_MASK_IMAGE_H_
+#define JSK_PERCEPTION_INVERT_MASK_IMAGE_H_
+
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <sensor_msgs/Image.h>
+#include <ros/ros.h>
+
+
+namespace jsk_perception
+{
+  class InvertMaskImage: public jsk_topic_tools::DiagnosticNodelet
+  {
+  public:
+    InvertMaskImage(): DiagnosticNodelet("InvertMaskImage") {}
+    virtual ~InvertMaskImage();
+  protected:
+
+    virtual void onInit();
+    virtual void subscribe();
+    virtual void unsubscribe();
+    virtual void invert(const sensor_msgs::Image::ConstPtr& mask_msg);
+
+    boost::mutex mutex_;
+    ros::Subscriber sub_mask_;
+    ros::Publisher pub_mask_;
+  private:
+  };
+}
+
+#endif

jsk_perception/plugins/nodelet/libjsk_perception.xml

@@ -143,6 +143,9 @@
   <class name="jsk_perception/UnapplyMaskImage" type="jsk_perception::UnapplyMaskImage"
          base_class_type="nodelet::Nodelet">
   </class>
+  <class name="jsk_perception/InvertMaskImage" type="jsk_perception::InvertMaskImage"
+         base_class_type="nodelet::Nodelet">
+  </class>
   <class name="jsk_perception/GridLabel" type="jsk_perception::GridLabel"
          base_class_type="nodelet::Nodelet">
   </class>

jsk_perception/sample/sample_invert_mask_image.launch

@@ -0,0 +1,81 @@
+<launch>
+
+  <arg name="gui" default="true" />
+  <arg name="launch_manager" default="true" />
+
+  <arg name="MANAGER" default="sample_manager" />
+  <node name="$(arg MANAGER)"
+        pkg="nodelet" type="nodelet" args="manager"
+        if="$(arg launch_manager)"
+        output="screen" />
+
+  <node name="image_publisher"
+        pkg="jsk_perception" type="image_publisher.py">
+    <rosparam subst_value="true">
+      file_name: $(find jsk_perception)/sample/kiva_pod_image_color.jpg
+      encoding: bgr8
+      publish_info: false
+    </rosparam>
+  </node>
+  <arg name="INPUT_IMAGE" default="image_publisher/output" />
+
+  <node name="mask_publisher"
+        pkg="jsk_perception" type="image_publisher.py">
+    <rosparam subst_value="true">
+      file_name: $(find jsk_perception)/sample/kiva_pod_mask.jpg
+      encoding: mono8
+      publish_info: false
+    </rosparam>
+  </node>
+  <arg name="MASK_IMAGE" default="mask_publisher/output" />
+
+  <node name="invert_mask_image"
+        pkg="nodelet" type="nodelet"
+        args="load jsk_perception/InvertMaskImage $(arg MANAGER)"
+        respawn="true" >
+    <remap from="~input" to="mask_publisher/output" />
+  </node>
+  <arg name="INVERTED_MASK_IMAGE" default="invert_mask_image/output" />
+
+  <node name="apply_mask_image"
+        pkg="nodelet" type="nodelet"
+        args="load jsk_perception/ApplyMaskImage $(arg MANAGER)"
+        respawn="true" >
+    <remap from="~input" to="$(arg INPUT_IMAGE)" />
+    <remap from="~input/mask" to="$(arg MASK_IMAGE)" />
+    <rosparam>
+      approximate_sync: true
+    </rosparam>
+  </node>
+
+  <node name="apply_mask_image_with_invert_mask_image"
+        pkg="nodelet" type="nodelet"
+        args="load jsk_perception/ApplyMaskImage $(arg MANAGER)"
+        respawn="true" >
+    <remap from="~input" to="$(arg INPUT_IMAGE)" />
+    <remap from="~input/mask" to="$(arg INVERTED_MASK_IMAGE)" />
+    <rosparam>
+      approximate_sync: true
+    </rosparam>
+  </node>
+
+  <group if="$(arg gui)">
+    <node name="image_view0"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="$(arg INPUT_IMAGE)" />
+    </node>
+    <node name="image_view1"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="$(arg MASK_IMAGE)" />
+    </node>
+    <node name="image_view2"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="apply_mask_image/output" />
+    </node>
+    <node name="image_view3"
+          pkg="image_view" type="image_view">
+      <remap from="image" to="apply_mask_image_with_invert_mask_image/output" />
+    </node>
+  </group>
+
+</launch>

jsk_perception/src/invert_mask_image.cpp

@@ -0,0 +1,88 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2022, 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/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the JSK Lab 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 "jsk_perception/invert_mask_image.h"
+#include <boost/assign.hpp>
+#include <jsk_topic_tools/log_utils.h>
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+
+namespace jsk_perception
+{
+  void InvertMaskImage::onInit()
+  {
+    DiagnosticNodelet::onInit();
+    pub_mask_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
+    onInitPostProcess();
+  }
+
+  InvertMaskImage::~InvertMaskImage() {
+  }
+
+  void InvertMaskImage::subscribe()
+  {
+    int queue_size;
+    pnh_->param("queue_size", queue_size, 1);
+    sub_mask_ = pnh_->subscribe("input", queue_size, &InvertMaskImage::invert, this);
+    ros::V_string names = boost::assign::list_of("~input");
+    jsk_topic_tools::warnNoRemap(names);
+  }
+
+  void InvertMaskImage::unsubscribe()
+  {
+    sub_mask_.shutdown();
+  }
+
+  void InvertMaskImage::invert(const sensor_msgs::Image::ConstPtr& mask_msg)
+  {
+    vital_checker_->poke();
+    cv::Mat mask;
+    try {
+      mask = cv_bridge::toCvShare(mask_msg, "mono8")->image;
+    } catch(std::exception &e) {
+      ROS_ERROR("cv_bridge exception: %s", e.what());
+      return;
+    }
+    cv::bitwise_not(mask, mask);
+    pub_mask_.publish(cv_bridge::CvImage(
+                        mask_msg->header,
+                        "mono8",
+                        mask).toImageMsg());
+  }
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS (jsk_perception::InvertMaskImage, nodelet::Nodelet);

jsk_perception/test/invert_mask_image.test

@@ -0,0 +1,19 @@
+<launch>
+
+  <include file="$(find jsk_perception)/sample/sample_invert_mask_image.launch">
+    <arg name="gui" value="false" />
+  </include>
+
+  <test test-name="test_invert_mask_image"
+        name="test_invert_mask_image"
+        pkg="jsk_tools" type="test_topic_published.py"
+        retry="3">
+    <rosparam>
+      topic_0: /invert_mask_image/output
+      timeout_0: 30
+      check_after_kill_node: true
+      node_names: [invert_mask_image,]
+    </rosparam>
+  </test>
+
+</launch>