fixing tf warnign for ground truth mode (nasa#454)

* fixing tf warnign for ground truth mode * adding header
luisa-mao · Mar 16, 2022 · 8706c4f · 8706c4f
1 parent 5e49689
commit 8706c4f
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/...on/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h b/...on/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h
@@ -60,6 +60,7 @@ class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerNodelet {
   void PublishLocState(const localization_common::Time& timestamp);
 
   std::string platform_name_;
+  ros::Time last_time_;
   boost::optional<Eigen::Isometry3d> pose_;
   boost::optional<Twist> twist_;
   int input_mode_ = ff_msgs::SetEkfInputRequest::MODE_TRUTH;

diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc
@@ -97,6 +97,11 @@ void GroundTruthLocalizerNodelet::PublishLocState(const lc::Time& timestamp) {
 
   // Also publish world_T_body TF
   const auto world_T_body_tf = lc::PoseToTF(*pose_, "world", "body", timestamp, platform_name_);
+
+  // If the rate is higher than the sim time, prevent repeated timestamps
+  if (world_T_body_tf.header.stamp == last_time_) return;
+  last_time_ = world_T_body_tf.header.stamp;
+
   transform_pub_.sendTransform(world_T_body_tf);
 }
 }  // namespace ground_truth_localizer