diff --git a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h index abde5fb7d..c9c86128b 100644 --- a/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h +++ b/aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h @@ -37,11 +37,14 @@ #include #include +#include #include #include #include #include #include +#include + 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(); diff --git a/aerial_robot_estimation/src/sensor/vo.cpp b/aerial_robot_estimation/src/sensor/vo.cpp index 78feaf2ca..77ff670ea 100644 --- a/aerial_robot_estimation/src/sensor/vo.cpp +++ b/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;