tf broadcast

Author: MoscowskyAnton

launch/extended_object_detection_oakd_example.launch

@@ -18,9 +18,9 @@
         <param name="subscribe_depth" value="true"/>
         <param name="rate_limit_sec" value="0.1"/>
         <param name="publish_image_output" value="true"/>
-        <param name="publish_markers" value="true"/>        
-        
-        
+        <param name="publish_markers" value="true"/>
+        <param name="broadcast_tf" value="true"/>                
+                        
         <remap from="camera/image_raw" to="/oak/rgb/image"/>
         <remap from="camera/info" to="/oak/rgb/camera_info"/>
         <remap from="depth/image_raw" to="/oak/stereo/depth"/>

src/extended_object_detection_node/eod_node.cpp

@@ -6,6 +6,7 @@
 #include <math.h>
 #include <visualization_msgs/MarkerArray.h>
 #include "geometry_utils.h"
+#include <geometry_msgs/TransformStamped.h>
 
 
 geometry_msgs::Vector3 fromCvVector(cv::Vec3d cv_vector){
@@ -43,7 +44,8 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
     nh_p_.param("publish_image_output", publish_image_output, false);
     nh_p_.param("use_actual_time", use_actual_time, false);
     nh_p_.param("publish_markers", publish_markers, false);
-        
+    nh_p_.param("broadcast_tf", broadcast_tf, false);
+            
     std::string object_base_path;
     nh_p_.getParam("object_base",object_base_path);
 
@@ -259,6 +261,33 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
     complex_objects_pub_.publish(complex_msg);
 #endif
 
+    if(broadcast_tf){
+        geometry_msgs::TransformStamped trsfrm;
+        trsfrm.header = header;
+        int id = 0;
+        std::string prev_name = "";
+        for( auto& bo : simples_msg.objects){            
+            if( prev_name != bo.type_name)
+                id = 0;
+            trsfrm.child_frame_id = bo.type_name + "_" + std::to_string(id);
+            trsfrm.transform = bo.transform;
+            transform_broadcaster_.sendTransform(trsfrm);
+            id++;
+            prev_name = bo.type_name;
+        }
+#ifdef USE_IGRAPH
+        for( auto& cbo : complex_msg.objects){
+            if( prev_name != cbo.complex_object.type_name)
+                id = 0;
+            trsfrm.child_frame_id = cbo.complex_object.type_name + "_" + std::to_string(id);
+            trsfrm.transform = cbo.complex_object.transform;
+            transform_broadcaster_.sendTransform(trsfrm);
+            id++;
+            prev_name = cbo.complex_object.type_name;    
+        }
+#endif                        
+    }
+    
     if(publish_markers){
         visualization_msgs::MarkerArray mrk_array_msg;    
         int id_cnt = 0;        
@@ -293,7 +322,6 @@ void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std
     frame_sequence++;    
 }
 
-
 extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( std::string name, int id,  eod::ExtendedObjectInfo* eoi, const cv::Mat& K){
     //ROS_INFO("Forming...");
     extended_object_detection::BaseObject bo;

src/extended_object_detection_node/eod_node.h

@@ -9,6 +9,7 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 #include <std_msgs/Header.h>
 #include <sensor_msgs/Image.h>
@@ -63,13 +64,15 @@ class EOD_ROS{
 #ifdef USE_IGRAPH
     ros::ServiceServer set_complex_objects_srv_;
 #endif
+    tf2_ros::TransformBroadcaster transform_broadcaster_;
 
     // params
     bool subscribe_depth;
     double rate_limit_sec;
     bool publish_image_output;
     bool use_actual_time;
     bool publish_markers;
+    bool broadcast_tf;
 
     // vars
     int frame_sequence;     
@@ -96,12 +99,11 @@ class EOD_ROS{
 
     visualization_msgs::Marker base_object_to_marker_arrow(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
     visualization_msgs::Marker base_object_to_marker_frame(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
-    visualization_msgs::Marker base_object_to_marker_text(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);
-    
+    visualization_msgs::Marker base_object_to_marker_text(extended_object_detection::BaseObject& base_object, const cv::Mat& K, std_msgs::Header header, cv::Scalar color, int id);        
     int find_simple_obj_index_by_id(int id);
 #ifdef USE_IGRAPH
     int find_complex_obj_index_by_id(int id);
-#endif
+#endif    
 
     // EOD
     eod::ObjectBase * object_base;