From 36c6c50e9f50a823f7f87e2929622d4fbfc66b87 Mon Sep 17 00:00:00 2001
From: cameron brown <52760912+cbrxyz@users.noreply.github.com>
Date: Tue, 10 Oct 2023 18:11:50 -0400
Subject: [PATCH] Limit TF_REPEATED_DATA messages by checking for repeated
 transforms, also remove dummy world join to rejoin map + base_link trees in
 simulation (#1097)

---
 .../simulation/subjugator_gazebo/urdf/sub8.urdf.xacro     | 7 -------
 mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp      | 8 ++++++++
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro b/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro
index 9290884dd..3f4517335 100644
--- a/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro
+++ b/SubjuGator/simulation/subjugator_gazebo/urdf/sub8.urdf.xacro
@@ -11,8 +11,6 @@
   <xacro:include filename="$(find mil_gazebo)/xacro/passive_sonar.xacro"/>
   <xacro:include filename="$(find mil_gazebo)/xacro/inclinometer.xacro"/>
 
-  <link name="world"/>
-
   <!-- Base link of sub -->
   <link name="base_link">
     <inertial>
@@ -33,11 +31,6 @@
     </collision>
   </link>
 
-  <joint name="dummy_joint" type="fixed">
-    <parent link="world"/>
-    <child link="base_link"/>
-  </joint>
-
   <!-- Sensors -->
   <xacro:mil_fixed_link name="front_stereo" parent="base_link" xyz="0.2559 0 0.1707" rpy="0 0 0"/>
   <xacro:mil_camera name="front_left_cam" parent="front_stereo" namespace="/camera/front/left" xyz="0 0.0445 0" rpy="0 0 0"/>
diff --git a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp
index d2ec5a6bf..953f0fcbd 100644
--- a/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp
+++ b/mil_common/gnc/odometry_utils/src/odometry_to_tf.cpp
@@ -4,7 +4,9 @@
 #include <tf/transform_broadcaster.h>
 #include <tf/transform_datatypes.h>
 
+#include <map>
 #include <pluginlib/class_list_macros.hpp>
+#include <string>
 
 namespace odometry_utils
 {
@@ -13,11 +15,17 @@ class odometry_to_tf : public nodelet::Nodelet
 private:
   ros::Subscriber odom_sub;
   tf::TransformBroadcaster tf_br;
+  std::map<std::string, ros::Time> _last_tf_stamps;
 
   void handle_odom(const nav_msgs::Odometry::ConstPtr& msg)
   {
     tf::Transform transform;
     poseMsgToTF(msg->pose.pose, transform);
+    if (_last_tf_stamps.count(msg->header.frame_id) && _last_tf_stamps[msg->header.frame_id] <= msg->header.stamp)
+    {
+      return;
+    }
+    _last_tf_stamps[msg->header.frame_id] = msg->header.stamp;
     tf::StampedTransform stamped_transform(transform, msg->header.stamp, msg->header.frame_id, msg->child_frame_id);
     tf_br.sendTransform(stamped_transform);
   }