[Estimation] stop sending TF with same timestamp

jsk-ros-pkg · Jan 5, 2025 · 0c40d05 · 0c40d05
1 parent 25e0f64
commit 0c40d05
Showing 1 changed file with 22 additions and 15 deletions.
diff --git 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<sensor_plugin::Imu>(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;
 }