[objectness] publish debug image

ros-perception · Apr 12, 2022 · ba92d8d · ba92d8d
1 parent c8bafe4
commit ba92d8d
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 10 deletions.
diff --git a/src/nodelet/objectness_nodelet.cpp b/src/nodelet/objectness_nodelet.cpp
@@ -52,6 +52,7 @@ class ObjectnessNodelet : public opencv_apps::Nodelet
 {
   std::string window_name_;
 
+  image_transport::Publisher img_pub_;
   image_transport::Subscriber img_sub_;
   image_transport::CameraSubscriber cam_sub_;
   ros::Publisher msg_pub_;
@@ -100,6 +101,7 @@ class ObjectnessNodelet : public opencv_apps::Nodelet
       std::vector<cv::Vec4i> objectnessBoxes_;
       cv::Mat frame_, debug_frame_;
       opencv_apps::RectArrayStamped rects_;
+      sensor_msgs::ImagePtr out_img_;
 
       // convert the image msg to cv object
       if (msg->encoding == sensor_msgs::image_encodings::BGR8)
@@ -145,13 +147,20 @@ class ObjectnessNodelet : public opencv_apps::Nodelet
           rect_.height = b[3] - b[1];
           rects_.rects.push_back(rect_);
           // draw rect in debug view
-          if (debug_view_)
-            cv::rectangle(debug_frame_, cv::Vec2i(b[0], b[1]), cv::Vec2i(b[2], b[3]), cv::Vec3i(0, 0, 255), 3);
+          cv::rectangle(debug_frame_, cv::Vec2i(b[0], b[1]), cv::Vec2i(b[2], b[3]), cv::Vec3i(0, 0, 255), 3);
         }
         // publish
         msg_pub_.publish(rects_);
       }
 
+      // publish image
+      if (!objectnessBoxes_.empty())
+        out_img_ = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, debug_frame_).toImageMsg();
+      else
+        out_img_ = cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::BGR8, frame_).toImageMsg();
+      img_pub_.publish(out_img_);
+
+      // draw debug window
       if (debug_view_)
       {
         cv::imshow(window_name_, debug_frame_);
@@ -160,8 +169,8 @@ class ObjectnessNodelet : public opencv_apps::Nodelet
     }
     catch (cv::Exception& e)
     {
-      NODELET_ERROR("Image processing error: %s %s %s %i\n", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
-      NODELET_ERROR("Please check whether you set the training path correctly at the same time\n");
+      NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+      NODELET_ERROR("Please check whether you set the training path correctly at the same time");
     }
   }
 
@@ -201,6 +210,7 @@ class ObjectnessNodelet : public opencv_apps::Nodelet
         std::bind(&ObjectnessNodelet::reconfigureCallback, this, std::placeholders::_1, std::placeholders::_2);
     reconfigure_server_->setCallback(f);
 
+    img_pub_ = advertiseImage(*pnh_, "image", 1);
     msg_pub_ = advertise<opencv_apps::RectArrayStamped>(*pnh_, "rects", 1);
 
     onInitPostProcess();

diff --git a/test/test-objectness.test b/test/test-objectness.test
@@ -12,18 +12,18 @@
       <arg name="debug_view" value="$(arg gui)" />
       <arg name="image" value="image_rect_color" />
       <arg name="training_path" value="$(find opencv_apps)/test" />
-      <arg name="node_name" value="objectness_default" />
+      <arg name="node_name" value="objectness" />
     </include>
 
     <!-- Test Codes -->
     <node name="objectness_saver" pkg="image_view" type="image_saver" >
-      <param name="filename_format" value="$(find opencv_apps)/test/objectness_default.png"/>
+      <param name="filename_format" value="$(find opencv_apps)/test/objectness.png"/>
       <param name="queue_size" value="5" />
-      <remap from="image" to="objectness_default/image" />
-      <remap from="objectness_default/camera_info" to="camera_info" />
+      <remap from="image" to="objectness/image" />
+      <remap from="objectness/camera_info" to="camera_info" />
     </node>
-    <param name="objectness_default_test/topic" value="objectness_default/image" />
-    <test test-name="objectness_default_test" pkg="rostest" type="hztest" name="objectness_default_test" >
+    <param name="objectness_test/topic" value="objectness/image" />
+    <test test-name="objectness_test" pkg="rostest" type="hztest" name="objectness_test" >
       <param name="hz" value="10" />
       <param name="hzerror" value="5" />
       <param name="test_duration" value="5.0" />