added image output

Author: MoscowskyAnton

launch/extended_object_detection_oakd_example.launch

@@ -12,15 +12,16 @@
 
         <param name="subscribe_depth" value="true"/>
         <param name="rate_limit_sec" value="0.1"/>
+        <param name="publish_output" value="true"/>
 
 
         <remap from="camera/image_raw" to="/rgb_stereo_publisher/color/image"/>
         <remap from="camera/info" to="/rgb_stereo_publisher/color/camera_info"/>
         <remap from="depth/image_raw" to="/rgb_stereo_publisher/stereo/depth"/>
         <remap from="depth/info" to="/rgb_stereo_publisher/stereo/camera_info"/>
 
-        <rosparam param="selectedOnStartSimple">[40]</rosparam>
-        <rosparam param="selectedOnStartComplex">[-1]</rosparam>
+        <rosparam param="selected_on_start_simple_objects">[40]</rosparam>
+        <rosparam param="selected_on_start_complex_objects">[-1]</rosparam>
 
     </node>
 

src/extended_object_detection_node/eod_node.cpp

@@ -64,6 +64,9 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
 
     // set up publishers
     simple_objects_pub_ = nh_p.advertise<extended_object_detection::SimpleObjectArray>("simple_objects",1);
+    
+    private_it_ = new image_transport::ImageTransport(nh_p_);
+    output_image_pub_ = private_it_->advertise("detected_image", 1);
 
     // set up message filters
     if( !subscribe_depth){
@@ -179,6 +182,8 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
         s_it->Identify(rgb, depth, frame_sequence);        
         //ROS_INFO("Adding...");
         add_data_to_simple_msg(&(*s_it), simples_msg, rgb.K);
+        if(publish_output)
+            s_it->draw(image_to_draw, cv::Scalar(0, 255, 0));
     }
 
     simples_msg.header = header;
@@ -187,6 +192,12 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
 
     simple_objects_pub_.publish(simples_msg);
 
+    if(publish_output){
+        sensor_msgs::ImagePtr detected_image_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_to_draw).toImageMsg();
+        output_image_pub_.publish(detected_image_msg);
+    }
+    
+    
     frame_sequence++;    
 }
 

src/extended_object_detection_node/eod_node.h

@@ -49,6 +49,8 @@ class EOD_ROS{
     boost::shared_ptr<RGBDSynchronizer> rgbd_sync_;
 
     ros::Publisher simple_objects_pub_;
+    image_transport::ImageTransport *private_it_;
+    image_transport::Publisher output_image_pub_;
 
     // params
     bool subscribe_depth;