[Estimation][VIO] add static tf from the world frame to the vo origin…

… frame

aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h

@@ -37,11 +37,14 @@
 
 #include <aerial_robot_estimation/sensor/base_plugin.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <kalman_filter/kf_pos_vel_acc_plugin.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/JointState.h>
 #include <spinal/ServoControlCmd.h>
 #include <std_msgs/Empty.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
 
 namespace sensor_plugin
 {
@@ -111,6 +114,8 @@ namespace sensor_plugin
     double reference_timestamp_;
     aerial_robot_msgs::States vo_state_;
 
+    tf2_ros::StaticTransformBroadcaster static_broadcaster_; // publish the transfrom between the work and vo frame
+
     void rosParamInit();
     void servoControl(const ros::TimerEvent & e);
     void estimateProcess();

aerial_robot_estimation/src/sensor/vo.cpp

@@ -265,8 +265,17 @@ namespace sensor_plugin
         /** step3: ^{w}H_{vo} = ^{w}H_{b'} * ^{b'}H_{vo} **/
         world_offset_tf_ = w_bdash_f * vo_bdash_f.inverse();
 
+        /* publish the offset tf if necessary */
+        geometry_msgs::TransformStamped static_transformStamped;
+        static_transformStamped.header.stamp = vo_msg->header.stamp;
+        static_transformStamped.header.frame_id = "world";
+        static_transformStamped.child_frame_id = vo_msg->header.frame_id;
+        tf::transformTFToMsg(world_offset_tf_, static_transformStamped.transform);
+        static_broadcaster_.sendTransform(static_transformStamped);
+
         tf::Vector3 init_pos = w_bdash_f.getOrigin();
 
+
         for(auto& fuser : estimator_->getFuser(aerial_robot_estimation::EGOMOTION_ESTIMATE))
           {
             string plugin_name = fuser.first;