Limit TF_REPEATED_DATA messages by checking for repeated transforms, …

…also remove dummy world join to rejoin map + base_link trees in simulation (#1097)

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"/>

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);
   }