From ffc9c8e2177041272b900afa7d6efd2253ad213f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Sat, 11 Jan 2025 23:51:58 +0900 Subject: [PATCH] [Estimation] stop sending TF with same timestamp (#650) Co-authored-by: Moju Zhao --- .../src/state_estimation.cpp | 37 +++++++++++-------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/aerial_robot_estimation/src/state_estimation.cpp b/aerial_robot_estimation/src/state_estimation.cpp index be3efd10b..698c4884c 100644 --- a/aerial_robot_estimation/src/state_estimation.cpp +++ b/aerial_robot_estimation/src/state_estimation.cpp @@ -84,6 +84,8 @@ void StateEstimator::initialize(ros::NodeHandle nh, ros::NodeHandle nh_private, void StateEstimator::statePublish(const ros::TimerEvent & e) { + static ros::Time prev_pub_stamp = ros::Time(0); + ros::Time imu_stamp = boost::dynamic_pointer_cast(imu_handlers_.at(0))->getStamp(); aerial_robot_msgs::States full_state; full_state.header.stamp = imu_stamp; @@ -159,21 +161,25 @@ void StateEstimator::statePublish(const ros::TimerEvent & e) baselink_odom_pub_.publish(odom_state); /* TF broadcast from world frame */ - tf::Transform root2baselink_tf; - const auto segments_tf = robot_model_->getSegmentsTf(); - if(segments_tf.size() > 0) // kinemtiacs is initialized - tf::transformKDLToTF(segments_tf.at(robot_model_->getBaselinkName()), root2baselink_tf); - else - root2baselink_tf.setIdentity(); // not initialized - - tf::Transform world2baselink_tf; - tf::poseMsgToTF(odom_state.pose.pose, world2baselink_tf); - geometry_msgs::TransformStamped transformStamped; - tf::transformStampedTFToMsg(tf::StampedTransform(world2baselink_tf * root2baselink_tf.inverse(), - imu_stamp, "world", - tf::resolve(tf_prefix_, std::string("root"))), - transformStamped); - br_.sendTransform(transformStamped); + /* avoid the redundant timestamp which induces annoying loggings from TF server */ + if (imu_stamp != prev_pub_stamp) + { + tf::Transform root2baselink_tf; + const auto segments_tf = robot_model_->getSegmentsTf(); + if(segments_tf.size() > 0) // kinemtiacs is initialized + tf::transformKDLToTF(segments_tf.at(robot_model_->getBaselinkName()), root2baselink_tf); + else + root2baselink_tf.setIdentity(); // not initialized + + tf::Transform world2baselink_tf; + tf::poseMsgToTF(odom_state.pose.pose, world2baselink_tf); + geometry_msgs::TransformStamped transformStamped; + tf::transformStampedTFToMsg(tf::StampedTransform(world2baselink_tf * root2baselink_tf.inverse(), + imu_stamp, "world", + tf::resolve(tf_prefix_, std::string("root"))), + transformStamped); + br_.sendTransform(transformStamped); + } /* COG */ /* Rotation */ @@ -186,6 +192,7 @@ void StateEstimator::statePublish(const ros::TimerEvent & e) tf::vector3TFToMsg(getVel(Frame::COG, estimate_mode_), odom_state.twist.twist.linear); cog_odom_pub_.publish(odom_state); + prev_pub_stamp = imu_stamp; }