adding markers

Author: MoscowskyAnton

src/extended_object_detection_node/eod_node.cpp

@@ -4,6 +4,7 @@
 #include <geometry_msgs/Vector3.h>
 //#include <geometry_msgs/Point.h>
 #include <math.h>
+#include <visualization_msgs/MarkerArray.h>
 
 geometry_msgs::Vector3 getUnitTranslation(cv::Point point, const cv::Mat& K){
     geometry_msgs::Vector3 unit_translate;
@@ -30,6 +31,7 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
     nh_p_.param("rate_limit_sec", rate_limit_sec, 0.1);
     nh_p_.param("publish_output", publish_output, false);
     nh_p_.param("use_actual_time", use_actual_time, false);
+    nh_p_.param("publish_markers", publish_markers, false);
 
 
     std::string object_base_path;
@@ -65,7 +67,9 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
     set_simple_objects_srv_ = nh_p_.advertiseService("set_simple_objects", &EOD_ROS::set_simple_objects_cb, this);
 
     // set up publishers
-    simple_objects_pub_ = nh_p.advertise<extended_object_detection::SimpleObjectArray>("simple_objects",1);
+    simple_objects_pub_ = nh_p_.advertise<extended_object_detection::SimpleObjectArray>("simple_objects",1);
+    if( publish_markers)
+        simple_objects_markers_pub_ = nh_p_.advertise<visualization_msgs::MarkerArray>("simple_objects_markers",1);
 
     private_it_ = new image_transport::ImageTransport(nh_p_);
     output_image_pub_ = private_it_->advertise("detected_image", 1);
@@ -202,6 +206,9 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
 
     simple_objects_pub_.publish(simples_msg);
 
+    if(publish_markers){
+        publish_simple_as_markers(rgb.K, header);
+    }
     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);
@@ -250,6 +257,31 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleOb
     return bo;
 }
 
+void EOD_ROS::publish_simple_as_markers(const cv::Mat& K, std_msgs::Header header){
+    visualization_msgs::MarkerArray mrk_array_msg();
+    //mrk_array_msg.header = header;
+    for(auto& so : selected_simple_objects){
+        int id_cnt = 0;
+        for(auto& eoi : so->objects){
+            mrk_array_msg.markers.append(eoi_to_marker(so, &eoi, K, header, cv::Scalar(0, 255, 0)),id_cnt++);
+            
+            
+        }
+    }
+    simple_objects_markers_pub_.publish(mrk_array_msg);
+}
+
+visualization_msgs::Marker EOD_ROS::eoi_to_marker(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id){
+    visualization_msgs::Marker mrk;
+    mrk.header = header;
+    
+    mrk.ns = so->name;
+    mrk.id = id;
+    
+    
+    return mrk;
+}
+
 int EOD_ROS::find_simple_obj_index_by_id(int id){
     for( int i = 0 ; i < selected_simple_objects.size() ; i++ ){
         if( selected_simple_objects[i]->ID == id )
@@ -260,9 +292,7 @@ int EOD_ROS::find_simple_obj_index_by_id(int id){
 
 bool EOD_ROS::set_simple_objects_cb(extended_object_detection::SetObjects::Request &req, extended_object_detection::SetObjects::Response &res){
 
-    if( req.remove_all && req.add_all ){
-        // do literally nothing
-    }
+    if( req.remove_all && req.add_all ){/*do literally nothing*/}
     else{
         if( req.remove_all ){
             selected_simple_objects.clear();    

src/extended_object_detection_node/eod_node.h

@@ -13,6 +13,7 @@
 #include <std_msgs/Header.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/CameraInfo.h>
+#include <visualization_msgs/Marker.h>
 
 #include "extended_object_detection/BaseObject.h"
 #include "extended_object_detection/SimpleObjectArray.h"
@@ -50,6 +51,7 @@ class EOD_ROS{
     boost::shared_ptr<RGBDSynchronizer> rgbd_sync_;
 
     ros::Publisher simple_objects_pub_;
+    ros::Publisher simple_objects_markers_pub_;
     image_transport::ImageTransport *private_it_;
     image_transport::Publisher output_image_pub_;
 
@@ -60,6 +62,7 @@ class EOD_ROS{
     double rate_limit_sec;
     bool publish_output;
     bool use_actual_time;
+    bool publish_markers;
 
     // vars
     int frame_sequence;     
@@ -77,9 +80,13 @@ class EOD_ROS{
     void detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header);
     bool check_time(ros::Time stamp);
     void add_data_to_simple_msg( eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K);
-    extended_object_detection::BaseObject eoi_to_base_object( eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
+    extended_object_detection::BaseObject eoi_to_base_object(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
     cv::Mat getK(const sensor_msgs::CameraInfoConstPtr& info_msg);
     cv::Mat getD(const sensor_msgs::CameraInfoConstPtr& info_msg);
+    void publish_simple_as_markers(const cv::Mat& K, std_msgs::Header header);
+    
+    visualization_msgs::Marker eoi_to_marker(eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
+    
 
     int find_simple_obj_index_by_id(int id);